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Preface 


This  study  was  part  of  a  continuing  effort  to  design 
a  missile  tracker  for  one  of  the  Air  Force  Weapons  Laboratory's 
laser  weapons  using  modern  estimation  techniques.  Earlier 
studies  demonstrated  the  advantages  of  using  an  extended 
Kalman  filter  which  uses  direct  infrared  sensor  data.  My 
efforts  were  intended  to  extend  this  design  effort  by  synthe¬ 
sizing  and  testing  four  alternative  filters  to  the  extended 
Kalman  filter. 

I  would  like  to  express  my  thanks  to  my  advisor  Dr. 

Peter  S.  Maybeck  for  his  motivation,  advice,  and  time,  and 
to  an  understanding  typist  Julie  Meyer.  Finally,  a  special 
thanks  to  my  wife  and  daughter,  Janine  and  Tyree,  for  their 
good  humor. 


Paul  R.  Leuthauser 


Table  of  Contents 


Page 

Preface . ii 

List  of  Figures .  v 

List  of  Tables . vii 

List  of  Symbols . viii 

Abstract .  x 

I.  Introduction  .  1 

Background .  1 

Problem  .  2 

Scope .  2 

Assumptions  .  3 

FLIR .  3 

Closed-Loop  .  3 

Basic  Model .  3 

Best  Performance .  4 

Approach .  4 

II.  Previous  Investigations .  6 

Benign  Tracking  Task .  6 

Air-to-air  Missile  Tracking  Task . 12 

Improved  Target  Intensity  Model  .  12 

Improved  Dynamics  Model  .  14 

Baseline  Extended  Kalman  Filter  .  IS 

III.  Alternative  Filters . 17 

Extended  Kalman  Filter  with  Bias  Correction  Term.  .  18 

Constant  Gain  Filters . 21 

Statistically  Linearized  Filter  .  22 

Evaluating  ’gXxTtjjTtT] . . 25 

Evaluating  h[x(ti) ,t^]xT(ti) .  .  30 

Algorithm  Summary  .  .  34 

IV.  Analysis  Methods  .  36 

Monte  Carlo  Study . 36 

Computational  Burden  Analysis  .  43 

V.  Results . 44 

Best  Performance . 45 


1 


Page 


Imperfect  Initial  Conditions . 49 

Low  Signal-to-noi se  Ratio  Performance  .  54 

Incorrect  Filter  Modelling  Performance . 54 

Statistically  Linearized  Filter  .  56 

Computational  Burden . 61 

VI.  Conclusions  §  Recommendations  .  65 

Conclusions . 65 

Extended  Kalman  Filter  with  Bias 

Correction  Term . 65 

Constant  Gain  Extended  Kalman  Filter  .  65 

Statistically  Linearized  Filter . 65 

Recommendations  .  66 

Bibliography  .  68 

Appendix  A . 70 

Appendix  B . 82 

Appendix  C . 85 

Appendix  D . 89 

Appendix  E . 127 

Appendix  F . 191 

Appendix  G . 249 


iv 


List  of  Figures 


gure 

1  Circular  Equal -intensity  Contours.  .  . 

2  Elliptical  Equal- intens ity  Contours.  . 

3  Region  for  Evaluating  P^  . 

4  Realizations  of  [xpeakUiJ,  ypeak^i-H 

5  Evaluating  . 

6  Access  of  Values  for  h[ Xpgg^ l tj ) , 

ypeak^i-)*  ci  3^ . 

7  Monte  Carlo  Study . 

8  Variance  Convergence  . 

9  Target  Intensity  Mismatch  Performance- 

Extended  Kalman  Filter  . 

10  Target  Intensity  Mismatch  Performance- 

Constant  Gain  EKF . 

A-l  Truth  Model  Reference  Frame . 

A-2  Instensity  Image  Orientation  . 


E-l 

Performance 

Plot 

-  Ca  se 

E-l .  . 

E-2 

Performance 

Plot 

-  Case 

E-2.  . 

E-3 

Performance 

Plot 

-  Ca  se 

E-3.  . 

E-4 

Performance 

Plot 

-  Case 

E-4.  . 

E-5 

Performance 

Plot 

-  Ca  se 

E-5.  . 

E-6 

Performance 

Plot 

-  Case 

E-6 .  . 

E-7  Performance  Plot  -  Case  E-7 


Figure  Page 

E-9  Performance  Plot  -  Case  E-9 . 171 

E-10  Performance  Plot  -  Case  E-10 . 175 

E-ll  Performance  Plot  -  Case  E-ll . 179 

E-12  Performance  Plot  -  Case  E-12 . 183 

E-13  Performance  Plot  -  Case  E-13 . 187 

F-l  Performance  Plot  -  Case  F-l . 195 

F-2  Performance  Plot  -  Case  F-2 . 199 

F-3  Performance  Plot  -  Case  F-3 . 207 

F-4  Performance  Plot  -  Case  F-4 . 215 

F-5  Performance  Plot  -  Case  F-5 . 219 

F-6  Performance  Plot  -  Case  F-6 . 223 

F-9  Performance  Plot  -  Case  F-9 . 229 

F-10  Performance  Plot  -  Case  F-10 . 233 

F-ll  Performance  Plot  -  Case  F-ll . 237 

F-12  Performance  Plot  -  Case  F-12  . . 241 

F-13  Performance  Plot  -  Case  F-13 . 245 

G-l  Performance  Plot  -  Case  6-1 . 252 

G-2  Performance  Plot  -  Case  G-2 . 254 

G-3  Performance  Plot  -  Case  G-3 . 256 

G-4  Performance  Plot  -  Case  G-4.  . . 258 

G-6  Performance  Plot  -  Case  G-6 . 260 

G-7  Performance  Plot  -  Case  G-7 . 262 


List  of  Tables 

Table  Page 

I  Truth  Model  Trajectories . 42 

II  Performance  Comparison,  Ideal  Conditions . 46 

III  Performance  Comparison,  Ideal  Conditions . 47 

IV  Bias  Correction  Term  and  Filter 

Measurement  Comparison . 48 

V  Initial  Condition  Mismatch  Recovery  Comparison.  ...  si 

VI  Filter  Acquisition  with  Bias  Correction  Term . 53 

VII  Performance  Comparison,  Low  S/N  Ratio  .  55 

VIII  Statistically  Linearized  Performance 

with  Varying  fx|z^ . 60 

IX  Computational  Burden .  62 

C-I  Turn  Trajectory  Kjs .  87 

C-II  Constant  Velocity  Trajectory  k|s  .  88 


List  of  Symbols 


t,  t 
x 

A 

X 

P 

'Id 

1 
R 
z 

2 
£ 

S/N 

Cx 

Zy 

K 

p 

a 

E{.} 

E{  •  |  • } 

_z(tj,GJj) 

6  ( t ) , 6 (t) 


t  ime 

State  vector 

Filter  state  estimate  vector 
Covariance  Matrix 

Discrete  process  noise  strength  matrix 
Maximum  intensity 

Covariance  of  measurement  noise  matrix 

General  measurement  vector 

Time  history  of  measurements  vector 

State  transition  matrix 

Signal  to  noise  ratio 

Dummy  variable 

Dummy  variable 

Dummy  variable 

Dummy  variable 

RMS,  variance,  or  standard  deviation 

Expected  value  operator 

Conditional  expected  value  operator 

Random  variable  defined  for  a  fixed  time 
and  stochastic  process  sample,a)i 

Delta  function 

Matrix  transpose 

Matrix  inverse 


vi  ii 


Time  derivative 


Superscripts 

x 


X 

tf 

tl 

Subscripts 

rF 

IT 

ti 

li-l 

xo 

xe 

Underline 

x 


Estimate 
After  update 
Before  update 

Filter 

Truth  value 

Current  sample  time 

Previous  sample  time 

Initial  value 

Inertial  reference  frame 

Vector  or  matrix 
Stochastic  process 


IX 


Abstract 


Four  alternative  filters  are  compared  to  an  extended 
Kalman  filter  (EKF)  algorithm  for  tracking  a  distributed 
(elliptical)  source  target  in  a  closed  loop  tracking  problem, 
using  outputs  from  a  forward  looking  (FLIR)  sensor  as  meas¬ 
urements.  These  were  1)  an  EKF  with  (second  order)  bias 
correction  term,  2)  a  constant  gain  EKF,  3)  a  constant  gain 
EKF  with  bias  correction  term,  and  4)  a  statistically  lin¬ 
earized  filter.  Estimates  are  made  of  both  actual 
target  motion  and  of  apparent  motion  due  to  atmospheric  jit¬ 
ter.  These  alternative  designs  are  considered  specifically 
to  address  some  of  the  significant  biases  exhibited  by  an 
EKF  due  to  initial  acquisition  difficulties,  unmodelled 
maneuvering  by  the  target,  low  signal- to-noise  ratio,  and 
real  world  conditions  varying  significantly  from  those  as¬ 
sumed  in  the  filter  design  (robustness) .  Filter  performance 
was  determined  with  a  Monte  Carlo  study  under  both  ideal 
and  non  ideal  conditions  for  tracking  targets  on  a  constant 
velocity  cross  range  path,  and  during  constant  acceleration 
turns  of  5G,  10G,  and  20G.  The  EKF  with  bias  correction  term 
demonstrated  nearly  identical  performance  to  the  EKF,  with 
a  cost  of  four  times  the  computational  burden.  The  constant 
gain  EKF  ideal  performance  was  not  as  good,  but  it  was  more 
robust  and  required  only  one-tenth  the  computational  burden. 
The  statistically  linearized  filter  was  unsuccessful  due  to 
the  particular  discretization  approximation  used  in 
evaluating  conditional  expectations  inherent  in  its  structure. 

x 
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I.  Introduction 


Background 

Recent  years  have  seen  the  advent  of  high  energy  laser 
weapons.  They  offer  the  distinct  advantage  of  being  able  to 
deliver  lethal  amounts  of  energy  to  a  target  essentially 
instantaneously.  An  efficient  implementation  of  this  weapon 
system  maintains  the  laser  beam  on  only  a  specific  part  of  the 
vehicle  until  it  is  destroyed  or  incapacitated,  rather  than 
using  enormous  amounts  of  energy  to  "paint"  the  entire  tar¬ 
get  vehicle  with  laser  energy.  Such  an  implementation  re¬ 
quires  both  precise  pointing  of  the  laser  and  accurate  track¬ 
ing  of  the  target.  It  is  the  tracking  portion  of  this  system 
that  is  the  subject  of  this  study. 

Previous  studies  (Refs  13;  6)  have  shown  the  advantage 
of  using  an  extended  Kalman  filter  as  a  tracker  because  of  its 
ability  to  extract  more  information  from  sensor  measurements 
than  does  a  currently  used  correlation  algorithm  (Refs  15;  16; 
17;  18).  First,  the  statistical  characteristics  of  the 
atmospheric  effect  on  the  infrared  wavefront  into  an  infrared 
sensor  can  be  modelled.  This  allows  the  filter  to  separate 
true  target  motion  from  atmospheric  jitter.  Second,  the 
dynamics  model  imbedded  in  the  extended  Kalman  filter  gives  it 
the  ability  to  predict  future  target  position.  Thus,  for  a 
sample  data  pointing  and  tracking  system,  the  tracker  is  able 
to  command  the  pointer  to  future  target  positions. 
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Problem 


The  Air  Force  Weapons  Laboratory  is  currently  using  a 
Forward  Looking  Infrared  (FLIR)  sensor  in  conjunction  with  a 
tracking  system  to  provide  accurate  target  position  estimates. 
Specifically,  the  ability  is  needed  to  track  air-to-air 
missiles  at  close  range  to  accuracies  considerably  better  than 
one  milliradian  standard  deviation.  In  this  dynamic  scenario, 
the  validity  of  a  first  order  extended  Kalman  filter  using 
FLIR  outputs  is  questionable  because  of  high  maneuverability 
and  the  low  signal-to-noise  ratio  environment.  As  a  result, 
alternative  filters  are  developed  in  this  study  which  also 
exploit  the  same  information  as  the  extended  Kalman  filter. 

Scope 

In  an  effort  to  provide  precise  target  state  estimates, 
several  filters  are  developed,  evaluated,  and  compared  as 
alternatives  to  an  extended  Kalman  filter.  These  filters 
include : 

1)  Extended  Kalman  Filter  with  Bias  Correction  Term 

2)  Constant  Gain  Extended  Kalman  Filter 

3)  Constant  Gain  Extended  Kalman  Filter  with  Bias 
Correction  Term 

4)  Statistically  Linearized  Filter 

Filter  performance  is  evaluated  by  tracking  simulated  trajec¬ 
tories  which  include  both  typical  and  worst  case  missile 
dynamics.  Such  a  worst  case  situation  is  the  one  alluded  to 
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previously,  that  of  tracking  an  accelerating  missile  in  a 
cluttered  background.  In  addition,  filter  robustness  to 
imperfect  initial  conditions  and  modelling  errors  is  addressed. 
Finally,  filter  performance  is  weighed  against  computational 
burden. 

Assumptions 

FLIR.  The  FLIR,  as  described  in  (Ref  6:p4),  provides  a 
frame  of  data  every  thirtieth  of  a  second  for  use  by  the  filter, 
thus  defining  a  30  Hz  data  sample  rate.  For  this  study,  an 
8-by-8  array  of  pixels  extracted  from  a  much  larger  FLIR  frame 
of  pixels  constitutes  a  single  measurement  array.  Computation¬ 
al  and  storage  limitations  are  responsible  for  reducing  the 
measurement  array  to  this  smaller  "tracking  window".  Finally, 
biases  in  alternating  rows  of  FLIR  data  (Ref  6 :p5)  were  assumed 
to  be  compensated  prior  to  the  data  reaching  the  filter. 

Closed  Loop.  The  tracker  is  assumed  to  be  operating 
within  a  closed  loop  pointing  and  tracking  system,  where  the 
laser  pointing  system  works  perfectly.  Thus,  the  system  can 
point  exactly  where  the  tracker  commands  it  within  each  sample 
period,  which  implies  that  the  pointing  system's  settling  time 
is  less  than  one  data  sample  period  (1/30  sec) .  This  dictates 
the  requirement  for  a  good  dynamics  propagation  model  within 
the  filter. 

Basic  Model.  Throughout  this  study,  the  filter  algo¬ 
rithms  are  based  on  a  linear  dynamics  model  and  a  nonlinear 
measurement  model.  The  dynamics  model  is  detailed  in  Section 
II,  and  it  is  implemented  the  same  in  all  the  algorithms.  It 
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is  the  method  of  dealing  with  the  nonlinear  measurement  that 
distinguishes  each  of  the  filters. 

Best  Performance 

Simulations  to  determine  each  filter's  best  performance 
were  conducted  under  rather  ideal  conditions.  First,  the 
signal-to-noise  ratio,  a  ratio  of  target  intensity  to  rms 
background  noise  intensity  on  the  FLIR  output,  was  held  high, 
but  at  a  physically  realistic  value.  Second,  artificial  a 
prior  knowledge  of  when  maneuvers  began  was  used  to  tune  the 
filter.  Adaptive  estimation  techniques  have  been  developed 
to  provide  filter  self-tuning  (Ref  6 : p 1 2 2  ;  12),  but  this  added 
complexity  w„  Id  only  cloud  the  basic  filter  structures  being 
evaluated.  Third,  perfect  knowledge  of  initial  conditions 
was  assumed  in  the  early  analyses  so  that  filter  performance 
did  not  suffer  from  transients  which  can  arise  with  imperfect 
initial  conditions.  This  study  also  addresses  the  performance 
degradation  due  to  imperfect  filter  modelling. 

Approach 

To  establish  a  foundation  for  this  study,  Section  II 
relates  a  history  of  previous  investigations.  An  extended 
Kalman  filter  is  derived  which  serves  as  the  baseline  tracker 
for  this  study.  The  extended  Kalman  filter  provides  levels 
of  performance  and  computational  burden  to  which  alternative 
filter  forms  may  be  compared. 

Next,  in  Section  III,  the  alternative  filters  are 
developed.  The  addition  of  bias  correction  terms  provides 
second  order  information  previously  ignored.  Often  the 


extended  Kalman  filter  produces  biased  estimates  from  harsh 
nonlinearities  because  they  are  based  on  only  a  first  order 
Taylor  series  representation.  Constant  gain  filters  promise 
not  only  lower  computational  burden,  but  they  may  also 
demonstrate  better  robustness  characteristics.  (Ref  19:p721) 
Finally,  the  statistically  linearized  filter  does  not  depend 
upon  a  truncated  Taylor  series  representation,  but  rather  re¬ 
lies  upon  a  statistical  approach  to  minimizing  the  error  in 
approximating  the  nonlinearity.  Such  an  approach  works  well 
when  the  filter's  confidence  in  its  estimate  is  low  enough  so 
that  the  true  state  value  may  lie  outside  the  linearized  region. 

To  complete  the  study.  Section  IV  describes  the  analysis 
methods  used,  Section  V  presents  the  results  of  this  research 
effort,  and  Section  VI  presents  conclusions  and  recommendations. 
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II .  Previous  Investigations 

Laser  pointing  and  tracking  is  a  topic  of  continuing 
research  at  the  AF  Institute  of  Technology.  This  section 
contains  a  summary  of  the  prior  work  which  forms  the  basis 
for  this  study. 

Benign  Tracking  Task 

As  originally  pursued  by  Mercier  (Ref  13) ,  the  accurate 
tracking  task  was  directed  at  long  range  targets,  where 
even  large  targets  appear  as  point  sources  of  infrared 
radiation.  This  simplified  modelling  the  target's  intensity 
pattern  on  the  FLIR  image  plane.  Also,  such  targets  appear 
to  the  tracker  to  exhibit  benign  dynamics,  which  kept  the 
dynamics  model  simple. 

By  considering  the  physics  of  wave  propagation  and 
optics  (Ref  14:pl5),  Mercier  modelled  the  target's  intensity 
pattern  on  the  FLIR  image  plane  as  a  bivariate  Gaussian 
function  with  circular  equal  intensity  contours  as  shown  in 
Fig  1.  The  model  was 

‘target'*-  r- ’  Imaxe*PC -^{Cx-*peak(t)  3*  * 

5  (2-1) 

C>’->’peak(t):|2,] 

where  (xpea^(t) »  /peak^^  locates  the  center  of  the  pattern 
relative  to  the  center  of  the  8-by-8  tracking  array,  Imax  is 


Circular  Equal -intensity  Contours 
Figure  I 
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T 


the  peak  intensity  value,  and  a  is  the  glint  dispersion. 

© 

I  and  a  were  assumed  to  be  known  and  the  apparent  target 
max  g 

location  is  actually  due  to  the  sum  of  target  dynamics,  at¬ 
mospheric  disturbances,  and  tracker  vibration.  Since  a 
ground-based  stable  platform  was  assumed,  vibration  effects 
were  ignored  and  thus 


VakCt)  '  Xdtt’  *  X*(t) 

Vak^)  =  *  ya(t) 


(2-2) 


where  the  subscript  d  and  a  represent  target  dynamics  and 
atmospheric  effects,  respectively. 

An  independent  first-order  Gauss-Markov  model  in  each 
direction  was  chosen  to  represent  the  benign  target  motion. 
Accounting  for  time-correlated  behavior,  the  model  is  pro¬ 
duced  by 


xd(t)  =-(l/Td)xd(t)  +  Wdx(t)  (2-3) 

where  T,  is  the  characteristic  correlation  time  of  the  target, 
a 

The  additive  noise  wdx  is  zero-mean  white  Gaussian  with  auto¬ 
correlation  function 


E{wdx(t)wdxCt+T)}  =  C2ad2/Td]<5(T)  (2-4) 

where  ad  is  the  rms  value  of  xd  (t)  .  A  similar  set  of 
relations  governs  yd(t) .  Appropriate  values  of  od  and  Td 
were  assumed  known. 

The  atmosphere  disturbance  terms,  xa(t)  and  ya(t)  ,  re¬ 
ferred  to  as  jitter,  have  been  shown  (Refs  1 ; 4 )  to  be  ade- 
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quately  modelled  as  the  output  of  a  third-order  linear  shaping 
filter  with  transfer  function 


G(s) 


K&>  1  (0  2  2 

Cs+oij)  (s+co2)  2 


(2-S) 


driven  by  unit-strength  white  Gaussian  noise.  The  values  used 
were  <^=14  rad/s,  <^  =  660  rad/s,  and  K  was  adjusted  to  obtain 
the  desired  rms  jitter  output  contribution.  Since  wi<<co2 
and  the  lower  frequencies  are  of  importance,  the  filter  ap¬ 
proximated  the  transfer  function  as  K&>i(s+&>i) 

The  measurement  observation  was  modelled  as  the  intensity 
pattern  of  (2-1)  corrupted  by  background  noise  and  inherent 
FLIR  errors.  Letting  z.,  (t.)  denote  the  measurement  available 

J  K  1 

at  time  t^  of  the  average  intensity  over  the  pixel  in  the  jtb 
row  and  ktb  column  of  the  8-by-8  array,  then 


zjk^ti-)  "  1//Ap^region  of  rtarget^x,^y ,t^d^xd?y 
jk™  pixel 

njk(V  +  bjk(V 


(2-6) 


where  Ap  is  the  area  of  one  pixel,  models  FLIR  noise 

effects,  and  b..(t.)  models  the  background  effects  on  the  jktb 

j K  1 

pixel.  Arraying  the  64  scalar  equations  (2-6)  in  a  single 
measurement  vector  yielded  the  measurement  model 


z(t.)  -  h[x(ti),ti]  +  n(ti)  +  b(tp  (2-7) 

T 

with  x  y^,  xa,  y  ]  as  the  output  of  a  four-state 

linear  dynamics  model  (reduced  from  eight  states  by  the  re¬ 
moval  of  co2  2  (s+a>2 )  2  from  (2-5)  in  each  axis  direction). 


FLIR  and  background  noises,  both  vectors  of  dimension  64, 
were  assumed  independent  of  each  other  so  that  their  variances 


added  for  a  given  pixel.  In  addition,  n(t^)  and  b(t^)  were 
each  assumed  to  be  both  spatially  and  temporally  uncorrelated 
and  stationary  processes. 

A  standard  extended  Kalman  filter  was  generated.  With 
four  states  and  64  measurements,  the  inverse  covariance  form 
(Ref  7:238-241)  of  measurement  update  was  employed  to  avoid  the 
computational  burden  of  large  matrix  inversions.  Also  due  to 
computational  considerations,  the  integral  in  (2-6)  was  ap¬ 
proximated  by  the  integrand  evaluated  at  the  center  of  the 
appropriate  pixel. 

The  filter  equations  used  for  propagating  the  state 

estimate,  x»  and  error  covariance,  P,  from  sample  time  t^_^  + 
(after  measurement  update  of  t^_^)  to  time  t^~  (before  meas¬ 
urement  update)  became: 


x(t.‘)  =  $x(ti.1+)  (2-8) 

PCtj')  =  iPCt..^)  (2-9) 


where  the  state  transition  matrix,  £,  and  input  covariance 


£d  = 


t  •  T 

/  1  ^(t.-T)^1 

zi-l  1 


(t.-T)dT 


(2-10) 


are  constant  and  readily  calculated  offline.  §  in  (2-10) 
represents  the  strength  of  the  continuous  time  stationary 
white  Gaussian  dynamics  driving  noise. 

After  combining  the  effects  of  n  and  b  in  (2-7) 
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t 


into  a  single  vector  v,  with  autocorrelation 


E{v(ti)vT(t:j)} 


ti=tj 


the  measurement  update  became, 


(2-11) 


P'1(ti+)  =  P'I(ti')  +  HT(ti)  R' 

1 Ct±)  H(tt) 

(2-12) 

P(ti+)  =  [P'1(ti+)]'1 

(2-13) 

K(ti)  =  P(tt+)  H T ( t i )  R'l(tt) 

(2-14) 

x(ti+)  =  x(ti')  +  K(ti)  (z(ti) 

-  h  [x(ti')  ,  ti]} 

(2-15) 

where  H(t^)  is  the  partial  of  h  with  respect  to  x,  evaluated  at 
x(t^  ),  and  R  1 (t^)  is  constant  and  is  generated  once  offline 
as  [1/R]  I_  using  (2-11). 

The  tracker  thus  formulated  consistently  outperformed  a 
correlation  tracker  under  identical  simulated  conditions. 
However,  robustness  studies  conducted  by  Harnly  and  Jensen 
showed  the  serious  problems  that  arise  when  the  filter  did  not 
correctly  model  true  conditions.  (Refs  11;  10)  With  an  eye 
toward  generating  a  tracker  capable  of  tracking  air-to-air 
missiles,  the  robustness  studies  yielded  insights  into  addi¬ 
tional  needed  aspects  in  the  tracker  design.  These  were 
summarized  as: 

1)  ability  to  estimate  size,  shape,  and  orientation  of 
the  target  image. 

2)  Online  estimation  of  target  intensity  height,  I  , 

luclX 

since  it  is  uncertain,  varying,  and  important  to 
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filter  residual  generation  and  tracking  performances. 
3)  Ability  to  predict  future  position  by  maintaining  at 


least  a  velocity  estimate  in  addition  to  a  position 
estimate  (acceleration  estimates  may  well  also  be 
required) ; 

4)  Adaptation  to  maneuvers  (detecting  a  maneuver  not 
predicted  by  the  filter,  via  residual  monitoring, 
and  responding  appropriately  through  gain  changing) . 
(Ref  9:p6) 


Air-to-Air  Missile  Tracking  Task 

Improved  Target  Intensity  Model.  The  task  of  tracking 
the  more  dynamic  air-to-air  missile  began  with  an  anal¬ 
ysis  of  real  FLIR  missile  image  data.  The  results  showed 
that  the  image  could  be  well  represented  by  a  bivariate 
Gaussian  intensity  pattern  with  elliptical  intensity  contours 
as  in  Fig  2.  (Refs6:p6;  9:p6;  12:pl73)  By  ignoring  small  angle 
of  attack  and  sideslip  angle,  the  semimajor  axis  of  the  ellipse 

was  aligned  with  the  velocity  vector.  Letting  Ax  and  Ax 

1  2 

be  measured  from  Upeak(t),  Ypeak^^  alon2  the  principal 
axes,  the  target  intensity  model  (2-1)  became 


1 target ^Ax*  *  Ax*’  Z)  *  Imaxexpf '?s[  (AXl/a§i ) '  + 

(A x2/a  )2]} 
g  2 


(2-16) 


where  I  ,  a  ,  and  o  were  treated  as  uncertain  parameters 
max’  gi  g2 

to  be  estimated  simultaneously  with  the  states. 


Estimates  of  a  and  a  were  generated  by  a  recursive 
gi  g  2 


Y 


Elliptical  Equal -intensity  Contours 
Figure  2 
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gradient  algorithm  for  minimizing  the  quadratic  cost 


C[Z(ti),  a]  =  (z(t.)  -  h[x(t.")f  ti;a]}T(z(ti)- 
h[xCti'),  ti;a]} 


(2-17) 


as  a  function  of  a,  the  vector  of  uncertain  parameters. 

I  ,  on  the  other  hand,  was  estimated  by  a  combination  of 

ULcL  A 

/x 

measurement  information.  The  estimate,  I,  was  calculated  by 
taking  the  maximum  observed  pixel  intensity  value,  correcting 
it  with  a  bias  function,  and  then  time  averaging  with  previous 
estimates.  (Ref  6;  9;  12) 

Improved  Dynamics  Model.  In  order  to  provide  the  ve¬ 
locity  estimate  required  for  orienting  the  elliptical  con¬ 
tours  and  to  generate  appropriate  tracking  commands  for  a 
laser  pointing  system,  a  simple  six-state  filter  was  imple¬ 
mented.  It  was  based  on  the  dynamics  model 


x(t)  =  v(t) 
v(t)  =  w(t) 


where  x(t)  is  target  position,  v(t)  is  target  velocity,  and 

T 

w(t)  is  white  Gaussian  noise  with  autocorrelation  E{w(t)w 
(t+x)}  =  £(t)6 (t) .  Q(t)  was  tuned  either  online  or  offline 
to  represent  target  maneuverability.  However,  as  the  tracker 
rotates  to  maintain  lock  on  the  target,  an  unmodelled  non- 
inertial  acceleration  is  created  on  the  FLIR  image  plane. 

Desiring  to  track  more  dynamic  targets,  Harnly  and 
Jensen  reduced  this  problem  by  introducing  an  eight-state 
filter  which  estimated  acceleration  in  the  FLIR  plan  based 
on  the  model 
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X(t)  =  v(t) 
v(t)  =  a ( t) 
a(t)  =  w(t) 

where  w(t)  now  models  the  derivative  of  the  target's  acceler¬ 
ation,  a(t) .  The  resulting  state  vector  was 

x  -  [xd,  yd,  xd,  yd,  xd,  Vd,  xa>  ya]T  (2-20) 

Harnly  and  Jensen  concluded  their  study  by  looking  at 
adaptive  tuning  and  maneuver  indicators.  The  dynamic  noise 
covariance  matrix,  £(t)  ,  was  adaptively  tuned  using  an 
approximation  to  maximum  likelihood  estimation.  However,  the 
filter's  resulting  self  tuning  ability  was  not  enough  to 
track  abrupt  harsh  maneuvers.  Such  maneuvers  required 
development  of  maneuver  detection  indicators  which  signaled  a 
need  for  additional  responses  within  the  filter.  Harnly  and 
Jensen  incorporated  two  responses,  (1)  increasing  the  gain 
by  reverting  to  an  acquisition  cycle  of  very  high  reini¬ 
tialized  P  and  high  and  (2)  using  updated  state  estimates 
for  reprocessing  the  previous  sample  period's  state  estimate 
time  propagation  and  for  future  propagation,  for  use  during 
an  ensuing  0.5  second  reacquisition  cycle. 

Baseline  Extended  Kalman  Filter 

The  starting  point  for  the  current  study  was  an  extended 
Kalman  filter  (EKF)  based  on  the  Harnly  and  Jensen  eight- 
state  filter  with  elliptical  equal- intensity  contours  in  the 
target  intensity  model.  Considering  the  scope  of  this 
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study,  only  the  adaptive  size  and  shape  estimation  through 

parameters  a  and  a  was  maintained,  thus  providing  a 
§!  g2 

straightforward  filter  as  the  standard  for  comparison  studies. 
Offline  tuning  of  Imax  and  Q(t)  replaced  the  adaptive  tuning  in 
order  to  provide  flexibility  in  establishing  best  performance 
standards.  Further,  the  rather  ad  hoc  maneuver  indicators 
and  associated  filter  responses  were  deleted,  so  as  not  to 
cloud  the  performance  comparisons  in  this  study.  Performance 
evaluation  of  this  filter  served  as  the  standard  for  compara¬ 
tive  evaluation  with  the  alternative  filters  pursued  in  the 
next  section. 

The  detailed  equations  for  this  filter  are  presented 
in  Appendix  A,  and  the  associated  computer  software  is  con¬ 
tained  in  Appendix  D. 
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Ill .  Alternative  Filters 

Inherent  in  the  formulation  of  the  extended  Kalman 
filter  in  Section  II  is  the  assumption  that  the  nonlinear 

A  _ 

target  intensity  function  (2-6)  can  be  linearized  about  x(t^  ) 
using  a  first  order  Taylor  Series  approximation.  This  linear¬ 
ization  process  assumes  that  the  process  errors  which  accumu¬ 
late  between  measurement  updates  remain  small.  One  method 
of  keeping  the  error  accumulation  small  is  to  decrease  the 
time  between  updates.  (Ref  2:p24)  However,  in  this  problem 
mechanization  of  the  FLIR  sensor  fixes  the  sample  rate.  Thus, 
other  approaches  must  be  considered  since,  as  Harnly  and 
Jensen  reported,  the  linearization  breaks  down  for  highly 
maneuvering  targets. 

As  a  result,  this  study  looks  at  several  alternative 
filters  in  hopes  of  gaining  improved  tracking  performance.  The 
first  of  these,  the  extended  Kalman  Filter  with  bias  correction 
term,  is  derived  from  a  Gaussian  second  order  filter.  Its 
additional  second  order  information  often  helps  the  EKF, 
which  neglected  higher  than  first  order  terms.  A  constant 
gain  EKF  and  a  constant  gain  EKF  with  bias  correction  term 
were  then  evaluated.  Often,  they  demonstrate  better  robust¬ 
ness  properties  than  the  standard  EKF.  Finally,  as  an  alter¬ 
native  to  these  two  conditional  moment  estimators,  a  statis¬ 
tically  linearized  estimator  is  developed.  Statistics  within 
the  filter  are  used  to  linearize  the  process  using  a  power 
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series  representation. 


Besides  tracking  performance,  computational  burden  is 
always  an  issue  due  to  online  computer  hardware  limitations. 
This  also  was  an  important  consideration  for  pursuing  the 
constant  gain  filters. 


Extended  Kalman  Filter  with  Bias  Correction  Term 

Approximate  conditional  moment  extimators,  which  includes 
the  linear  Kalman  Filter  and  linearized  extended  Kalman  filter, 
provide  an  estimate  which  is  an  approximate  evaluation  of  the 
conditional  mean 


E{x(ti)|Z£ti.i)  =  Ii-i) 


(3-la) 


x(ti/ti)  £E{x(ti)|  Z(ti)  =  Z.}  (3-lb) 

where  (3-la)  is  the  estimate  after  propagating  from  time  t^_1 
to  t^  and  (3-lb)  is  the  estimate  after  incorporating  the  meas¬ 
urement  at  time  t^.  These  two  values  are  also  referred  to  as 
x(t^’)  and  x(t^+)  respectively.  Exact  evaluation  of  these 
conditional  expectations  requires  knowledge  of  the  entire 
conditional  density  functions 


£x(ti)|  Z(ti.1)C^1^-  i-13 
£x(ti)IZ(ti)  i} 


(3- 2a) 
(3- 2b) 


which  includes  all  higher  moments  as  well  as  the  conditional 
mean  and  covariance.  (Ref  8:Chapt  12)  In  the  Kalman  filter 
for  a  problem  defined  by  linear  dynamic  and  measurement  models, 
driven  by  white  Guassian  noise,  the  first  two  moments  completely 
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describe  the  density  function  which  is  Gaussian.  The  result¬ 
ing  conditional  mean  is  then  the  optimal  estimate  from  a 
Bayesian  viewpoint.  (Ref  7:p215)  However,  for  nonlinear 
processes,  the  optimal  estimator  will  generally  be  infinite 
dimensional.  The  Gaussian  second  order  filter  assumes  that 
the  conditional  density  is  nearly  Gaussian,  so  that  third  and 
higher  odd  central  moments  are  essentially  zero,  and  that 
fourth  and  higher  even  central  moments  are  expressible  in 
terms  of  the  covariance.  In  this  study,  sixth  and  higher  even 
moments  are  also  neglected.  This  yields  an  algorithm  that 
does  not  require  evaluation  of  an  infinite  number  of  moments. 

Since  a  iinear  dynamics  model  was  assumed  for  this 
tracker,  attention  was  confined  to  the  nonlinear  measurement 
update.  Based  on  work  by  Kramer  and  Jazwinski,  one  approx¬ 
imation  technique  for  evaluating  (3-2)  is  presented  in  (Ref 
8:Chapt  12).  This  approach  assumes  that  the  mean  and  covar¬ 
iance  after  update  are  expressible  as  a  power  series  in  the 
residual , 


(z(t.)  -  l(ti-)}  =  (z(ti)-E[z(t.) |z(ti.1)  = 

(3-3) 

z(ti-i,.)]> 

which  is  truncated  at  first  order  terms: 

x(t . +)  =  a  +  a  { z  ( t . )  -  z(t.-)}  (3-4a) 

sr  1  =r o  — !  —  1  ST  1 

P(t.+)  =  b  +  b  {  z  ( t .  )  -  z  ( t .  ” )  }  (3- 4b) 

«r-  1  — 0  ~ 1  —  1  ST  1 


The  coefficients  in  (3-4)  are  then  evaluated  using  Bayes  Rule 
and  truncated  Taylor  Series  representations.  This  evaluation 
is  described  in  detail  in  (Ref  8:Chapt  12).  The  resulting 


Gaussian  second  order  (GS)  filter  is  further  modified  by 
setting  b  in  (3-4b)  to  zero.  This  eliminates  a  random 

~i 

residual  forcing  function  which  allowed  negative  computed 
values  of  P(t^+).  The  resulting  measurement  update 
equations  are: 


AggCtj)  -  H[x(ti'),  ti]  P(t.‘)  H^xCt."),  ti]  + 

KosC'i)  *  P(ti')HT  [x(ti'),t1]  ags-‘  (tp 
x^*)  =  x(ti")+KGS(ti)  fli-h[x(ti‘)  ,ti]  - 

V'i’^ 

P(ti+)  -  PCti’D-KcsCt^HCxUi'),  ti]P(ti‘) 


with  the  defining  relations 

H[x(t. ') ,t .  1  =  3-[-’ ti 3 

3x 

2 


"^ktelement 


=iitr{ - 5—5 - 


3xz 


32h^xC(ti) »ti] 


3x‘ 


"^k  ^element 


4.  ,a!hk[i'ti1 

=ijtr{ - 


th 


(3-5) 

(3-6) 

(3-7) 

(3-8) 


(3-9) 

(3-10) 


(3-11) 


where  h,[x,t.]  is  the  k  element  of  h[x,t.]. 

K  —  1  “  1 

The  structure  of  this  algorithm  is  similar  to  that  of 
the  EKF  in  Section  II.  Note  however,  that  this  form  requires 
additional  burdensome  second  moment  calculations.  Also  new 

A 

to  the  structure  is  the  vector  b  (t.)  in  the  residual  gener- 

— m  i 
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ation.  A  first  order  EKF  often  generates  biased  estimates 

due  to  neglected  nonlinearities,  but  the  addition  of  the 

second  order  information  contained  in  b  (t.)  reduces  the 

— m  1 

/v 

bias ,  hence  t^(tj)  is  the  "bias  correction"  term.  (Ref 
3  :p507) 

In  order  to  make  use  of  second  order  information  with¬ 
out  incurring  the  computational  burden  of  the  full-scale 
Gaussian  second  order  filter,  just  the  bias  correction  term 
is  added  to  a  first  order  EKF.  Specifically,  the  residuals 
are  generated  as  in  (3-7)  ,  but  the  filter  gain  and  covariance 
are  computed  as  in  the  EKF.  The  performance  improvement  from 
such  a  filter  over  that  of  an  EKF  is  related  to  the  harsh¬ 
ness  of  the  nonlinearity.  Thus,  substantial  enhancement  is 
expected  when  large  second  partial  derivative  values  de¬ 
velop  . 

As  implemented  for  this  study,  the  baseline  EKF  was 
revised  to  include  the  additional  bias  correction  terms. 

The  second  partial  derivatives  were  computed  in  subroutine 
MEASF  (see  Appendix  B  and  D)  at  the  same  time  as  the  first  partial 
derivatives.  An  additional  subroutine,  BIASCT,  performed 
the  matrix  multiplication  and  trace  operation  needed  in 
(3-11). 

Constant  Gain  Filters 

For  a  time-invariant  nonlinear  process  acting  along 
a  constant  nominal  state  trajectory,  the  linear  perturbation 
equations  are  time  invariant.  The  gain,  K(t^) ,  associated 
with  the  linearized  filter  for  such  a  trajectory  can  reach  a 
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steady  state  value,  Ess*  (Ref  8:Chapt  9)  Then  the  constant- 
gain  EKF  for  this  study  is  defined  by 

x(t.)  -  £x(ti+)  (3-12) 

x(t.+)  =  x(ti")  +  Kss(zi-h[x(ti')]}  (3-13) 

Likewise,  the  constant-gain  EKF  with  bias  correction  terms  is 
defined  by  (3-12)  using  the  steady  state  P(t^  )  and 

x(ti+)  =  x(ti")  +  Kss(zi-h[x(ti") ]  -  bm(ti')}  (3-14) 

Notice  that  these  filter  forms  do  not  require  knowledge  of 
the  conditional  covariance,  P(t  •’)  ,  thus  significantly  reducing 
computational  burden.  In  addition,  they  may  also  demonstrate 
good  robustness  characteristics  since  their  gain  calculation 
is  not  effected  by  filter  mismodelling .  (Ref  19:p721) 

This  study  used  a  simple  procedure  for  obtaining  Ks$. 
First,  the  filter-computed  gain  history,  K(t/),  was  recorded 
during  Monte  Carlo  simulation  runs.  Then,  a  Kss  was  chosen 
based  on  these  values  in  hope  of  gaining  good  performance 
for  both  the  5g  and  lOg  trajectories  described  in  the  next 
section  (see  Appendix  C) . 

An  alternative  method  for  obtaining  Kss  is  applying 
pole  placement  or  more  extensive  eigenvalue/eigenvector 
assignment  techniques  to  the  linearized  perturbation  equat¬ 
ions.  This  method  was  not  pursued  in  this  study. 

Statistically  Linearized  Filter 

In  contrast  to  dealing  with  the  nonlinear  measurement 
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function  h[x(t.),t.]  in  (2-6)  by  linearizing  with  a  first 
order  Taylor  series  approximation,  the  statistically 
linearized  (SL)  filter  approximates  h[x(ti),ti]  with  the 
first  two  terms  in  a  power  series 

h[x(ti),ti]  =  h0(ti)  +  H(ti)x(ti)  (3-15) 

with  ho(t^)  and  H(t^)  determined  through  an  optimization  tech¬ 
nique.  As  a  result,  h[x(t^) ,  t^]  need  not  be  differentiable 
with  respect  to  x(t^)  as  required  for  the  Taylor  series 
representation.  However,  determining  the  coefficients 
hQ  (t^)  and  H(t^)  requires  the  evaluation  of  several  condi¬ 
tional  expectations.  Without  the  assumptions  made  in  the 
following  development,  this  technique  could  become  infinite 
dimensional . 

The  representation  in  (3-15)  can  be  optimized  with 
respect  to  the  error  variable 


e(t)  =  hCxtt^,  ti]  -  [h  (t^  +  H(ti)x(ti)]  (3-16) 

so  that  the  assumed  form  has  minimum  mean  square  error,  i.e. 
minimum  value  of: 

J  *  E(eT(t)We(t) ! Z(tA)  =  Z^}  (3-17) 

where  W  is  a  general  weighting  matrix.  (Ref  8:Chapt  12)  Eval¬ 
uating  the  expectation  in  (3-17)  requires  knowledge  of  the 
conditional  density  function  (3-2b).  Conducting  the  opti¬ 
mization  yields  the  coefficients 
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(3-18) 


This  approximation  then  yields  measurement  update  equations  at 
time  t^  of 

ISL(V  =  P(ti'3!lTCti)[H(ti)P(ti-)HT(ti)+R(ti)]"1  (3-22) 

x(t.  +  )  =  x(ti-)+KSL(ti){z(ti)-h[i?^Tr^  (3-23) 

P(ti+)  =  P(ti*)-KSL(ti)H(ti)P(ti')  (3-24) 


with  H(t^)  given  by  (3-19).  The  conditional  expectations 
hx  ,  h»  and  x  in  the  above  relationships  are  computed  by 
assuming  that  the  conditional  density  function  (3-2b)  is 

A 

Gaussian  with  mean  x(t^  )  and  covariance  P(t^~)  ,  as  gener¬ 
ated  by  the  previous  propagation. 

Notice  the  structural  similarity  of  (3-22,  23,  24)  to 
the  EKF  update  equations  (2-12,  13,  14,  15),  where  now  the 
statistically  optimized  H(t^)  replaces  the  matrix  of  partial 
derivatives,  H(t^  ).  This  structure  accounts  for  variations 

A 

in  x(t^)  away  from  x(t^  ) ,  which  invalidate  first  order 
Taylor  series  representations.  As  a  result,  better  per- 
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forraance  is  expected,  especially  for  cases  which  have  large 
error  covariance  magnitudes. 


Evaluating  h[x(t^),t^].  As  noted  before,  this  filter 
requires  evaluating  several  conditional  expectations.  Look 
first  at  evaluating  the  term  h^  for  k  =  1,  2,  3,.  .  .,64 


VJ'V-V  =  -£^[1/Ap  "region  of  ^arget^V 

jktd  pixel 

4x2,tl]dxdy]f(x^iy^k)1^(Ex,?y,|Z1)dcJ!d5y 


(3-25) 


where  itarget  is  defined  in  (2-16).  The  approximate  numerical 
evaluation  of  (3-25)  evolved  from  a  physical  interpretation. 
First,  h[x(tji )  , t^]  takes  on  different  values  for  each 
realizations  of  x(t^)  .  Evaluating  h[x(ti) ,  t^]  however, 
requires  only  the  position  state  information  contained  in 

1"  Y\ 

Xpeak(ti^  and  ^peak^i^  '  T^us  f°r  each  l  realization  of 

i(ti),  x^Ctp,  [*peak(ti)-  >peak(ti)]f  is  formed  and 
h^xpeak(ti^ »  ypeak(tP  ’  ti^£  is  then  evaluated.  Second,  the 
conditional  probability  density  function,  assumed  Gaussian, 
is  generated  and  integrated  to  yield 


P£^Xpeak^i  ^  ’  ypeak^i  xpeak’  ypeak-^ 

ff  ffx  y  5[2(5x,5yl  Ii-i)d5xd^y  C3'26) 

At  (-xpeak,ypeakJ  >-  x  y  11  x  y 

where  is  an  infinitesimally  small  region  surrounding 

Opeak^P  ’  ypeak(ti^£*  P£  is  then  the  appropriate  proba¬ 
bility  that  [xpeak(ti),  ypeakCti)]^  locates  the  peak  of  the 
true  apparent  target  intensity  function.  The  final  step  in  thi 
interpretation  moves  through  realizations  x^(t^)  ,  evaluating 


» 


— Cxpeak^ti^  ’  ypeak^i^  ’ ti^£  and  P£  ■  Multiplying  these  two 
values  together  for  each  realization  and  then  summing  over 
an  inifinite  number  of  x^(t^)  values  approximates  (3-25). 


hcxjt^.t^  :  i  p 
1  =  1 


— 1-Xpeak^i^  '^peak^i^ 


(3-27) 


However,  implementing  such  a  summation  is  impractical  and  so 
a  discrete  approximation  is  pursued.  The  approximation  will 
seek  to  use  the  finite  pixel  area  as  an  appropriate  region 
in  order  to  exploit  the  EKF  measurement  software  based  on 
one  pixel  discretizations.  This  three  step  approach  led  to 
the  algorithm  in  Appendix  C,  whose  detailed  development 
follows . 

The  first  step,  evaluating  h[x(t^)  ,t^]  at  each  [xpeajc(ti)» 
Ypeak^i)^*  uses  the  same  software  which  evaluates  h[x(t^),t^] 
in  the  EKF.  As  before,  the  value  of  the  intensity  function 
(2-16)  at  the  pixel's  center  approximates  the  average  over  the 
entire  pixel. 

The  second  step,  evaluating  P^ ,  follows  a  different 

approach.  Generating  the  Gaussian  conditional  density 

function  imbedded  in  (3-2a),  f,  first  requires 

^  peak’ypeakJ ' — 

the  conditional  covariance  and  mean,  P(t^)  and  x(t^~) .  Using 
the  definition  of  Xpga^Ct)  and  ^peak^  (^-2)  and  the  state 
vector  (2-20)  transforms  the  desired  density  function  into  the 
FLIR  x-y  coordiante  frame. 


Vak(t)  '  *d(t)  *  xaCt) 

=[10000010]  x ( t)  (3-28a) 

=  [Tx]  x(t) 
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(3- 28b) 


T 


ypeak(t)  =  yd(t)  +  ya(t) 

=[0100000  l]x(t) 

=  [Ty.]x(t) 

The  combination  of  states  in  (3-28)  also  combines  the  atmos¬ 
pheric  and  dynamic  terms  of  P(t^’)  into  separate,  single 
sigma  values  for  both  the  x  and  y  directions. 


o5  «  [T„]P(t.*)[T  ]T  (3 - 29a) 

xpeak  x  ~  1  x 

Cy  -  [Tv]P(t.')[T  JT  C 3 - 2 9b ) 

'  peak  y  y 

The  resulting  bivariate  Gaussian  distribution  centered 
at  ^peak(ti*^’  ^peak^i simplifies  by  assuming  the  stoch¬ 
astic  processes  associated  with  the  FLIR  x  and  y  directions 
to  be  uncorrelated.  The  covariance  matrix  associated  with 

fr  then  becomes 

*•  peak ,ypeak-'  I  ~ 

(3-30) 


peak 

0 


peak 


This  allows  the  evaluation  of  fr  , , 7  to  be  computed 

upeak’ypeakJ  l£ 

as  the  product  of  the  two  one-dimensional  Guassian  distrib¬ 
utions 

fxiZ(5lIl-l)“li/2l,ax  ]exp{-ii[C-x 
~  peak 

1/oJ  )  (3'51 

peak 
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fy|Z^Hi-l)’C1/2™ypeak^1(P(-'>>:»-)'peak<ti_>]2- 


l/o *  } 

ypeak 


c  3 -  31b) 


Note  that  this  assumption  can  be  verified  by  computing  the 

off-diagonal  terms  in  (3-30)  as  [T  ]P(t.  )[T  ]T.  Consist- 

x  x  y 

ently,  the  diagonal  terms  in  (3-50)  are  an  order  of  magnitude 
larger  than  the  off-diagonal  correlation  terms,  which  justifies 
such  as  assumption. 

A  second  assumption  further  eases  the  computational 
burden  of  evaluating  f 


,  , | ? .  Examining  the  sigma 

^peak’^peak-1  ’  — 


values,  a 


and  a 


ypeak  xpeak 


,  in  the  results  from  the  other 


filters  showed  how  large  a  region  had  significant  probabil¬ 
ity  of  containing  Cx  k(ti) »ypeak^ti^ *  This  examination 
found  typical  3a  values  of  0.97  pixels  in  the  x  direction 
and  0.72  pixels  in  the  y  direction.  Thus,  the  evaluation  of 
P£  is  limited  to  the  pixel  containing  [  xpeak(  t  i" )  >ypeak(t  j," )  ] 
and  its  eight  nearest  neighbor  pixels  as  shown  in  Fig  3. 

The  third  and  final  step  in  evaluating  (3-25T  is  an 
implementation  of  the  summation 


hCxCtp.ti]*  £  Pz(t4)  •  h[xpeaktti),ypea^ti),t1]i 


(3-32) 


where  the  summation  index  l  represents  the  nine  pixels  of 
interest.  The  number  of  realizations  of  [x  eak(ti) 
is  limited  to  nine  in  consideration  of  computational  burden. 
In  line  with  the  approximately  zero-mean  error  assumption, 
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8- by-8 


Arroy  of 


Pixels 


ion  for  Evaluating 
Figure  3 


this  study  used  for  its  nine  points  [x  ,  (t • ' ) , v  ,  ft*' 
and  one  point  in  each  of  the  neighboring  pixels  corresponding 
to  a  one  pixel  shift  in  the  x  and/or  y  directions  from 

A  A 

^peak^i  ^ ’^peak^i  These  nine  realizations  are  shown 

in  Fig  4.  Thus  an  appropriate  region  in  (3-2a)  is  the 
pixel  itself.  Fig  5  summarizes  the  evaluation  of  the 
values.  Integrations  are  approximated  by  evaluating  the 
function  in  (3-31)  at  the  pixel  centers,  and  as  a  final  step, 
the  individual  pixel  probabilities  are  normalized  so  that 
the  conditional  probability  density  function  has  unit  volume 
over  the  nine  pixel  area.  A  final  computational  consideration 
for  (3-32)  evaluates  h[xpeak(ti)  ,  ypeakCti)  ,  t^  only  once 
for  a  10-by-10  pixel  array  centered  about  CX  ak(tp ,  ^peak 
(t^  )]  at  each  time  t^.  The  additional  rows  and  columns 
are  needed  to  evaluate  the  terms  in  (3-32)  associated  with 
the  eight  nearest  neighbor  pixels.  The  appropriate  8-by-8 
array  values  for  hCxpeak(ti)  ,  y^^O^)  ,  t.^  are  then 
accessed  for  each  iteration  in  (3-32)  as  shown  in  Fig  6. 

Evaluating  h[x(t^) , t^]x^(t^) .  Consider  next  the 


conditional  expectation 

T  1  oooo  T 

h[x(t . ) ,t . lx1 (t . )  =  ff  h[x(t,),t.]x  (t.) 

~  ±  ~  -L  -co-co  -  X  ±  ± 

Seak-Wi^*'5ylli,d^ 


(3-33) 


Intuitively,  a  summation  similar  to  (3-32)  seems  appropriate 

_  9 


<x 


Figure  4 
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Evaluating  (| 
Figure  5 


where  P£  and  hCxpeak(ti) »ypeak(ti) ,t±\  are  calculated  as  in 
the  previous  section.  The  realizations  of  the  state  vector, 
x/ctj),  are  based  on  the  points  Cx^Ctj), 

However,  each  unit  pixel  shift  of  either  xpeak(tj_)  0T  ^peak^i^ 
is  a  combination  of  the  atmo.spheric  and  dynamic  states  through 
the  transformation  (3-28).  Formulating  x^^(t^)  requires  an 
indication  of  how  much  of  tne  unit  pixel  shift  is  caused 
by  dynamics  and  by  atmospheric  jitter.  Such  indicators  are 
the  values 


where  the  sigma  values  are  the  square  roots  of  the  appropriate 

diagonal  terms  of  P(t^),  and  Sxd  and  5xa  or  5yd  and  <Sya  are 

T 

assumed  of  the  same  sign.  As  an  example,  Xg  (t^)  associated 
with  Cxpeak(ti)»ypeakCti)]9  in  Fig  4  would  be 

x/Ct,)  =  x1(ti')  +[5xd  -<Syd  0  0  0  0  jxa-<Sya]  (3-36) 

Note  that  pixel  shifts  up  and  shifts  right  are  considered 
positive . 

Algorithm  Summary.  This  study  implements  the  statis¬ 
tically  linearized  filter  by  integrating  the  coefficient 
H(tp  and  predicted  measurement  h[x(td)  ,  t^]  into  the  ex- 
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isting  EKF  software,  replacing  H(t^)  and  ]lCx(t^")  .t^  , 
respectively.  Subroutine  STATLN  in  Appendix  C  computes  the 
necessary  conditional  expectations  and  H(t^)  based  on  the 

A 

propagated  state,  x(t^’),  and  conditional  covariance,  P(t^  ). 
An  extension  to  subroutine  MEASF  provides  the  10-by-10  meas¬ 
urement  array  as  described  previously. 

As  a  summary  of  the  statistically  linearized  filter, 
the  sequence  of  steps  within  STATLN  follows: 

A  A 

1)  locate  pixel  containing  Cxpeak(ti") ,ypeak(ti") ] 

2)  calculate  axpeak  and  °ypeak  from  P(t.") 

3)  evaluate  P^  l =  1 ,  2,  3,  ...,  9 

— - - - - - -  9 

4)  evaluate  h[x(ti),ti]  as  P£*hCxpeakCti-l  > 

5)  calculate  6xa,  6x^,  <5ya>  anc*  5 Y ^  from  P(t^  ) 

6)  evaluate  h[x(fj,)  , t i U x T C t ^ )  as  t  P^_ •  hC xpeak ( t i )  , 

CV 

7)  evaluate  tf(t^)  using  equation  (3-19) 

Equations  (3-22)  through  (3-24  define  the  measurement 
update,  but  as  in  the  EKF  it  is  implemented  in  the  inverse 
covariance  form. 
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IV.  Analysis  Method 

The  primary  goal  of  analyzing  the  filters  in  this  study 
is  to  evaluate  their  relative  performance  (i.e.  the  filter's 
ability  to  track  targets  in  varying  conditions) .  This  is 
accomplished  through  a  Monte  Carlo  study.  When  comparing 
several  filters,  however,  an  analysis  should  also  address  the 
costs  associated  with  obtaining  desired  performance  levels,  so 
computational  burden  is  also  evaluated. 

Monte  Carlo  Study 

Evaluating  a  filter's  performance  requires  a  description 
of  the  estimation  errors  committed  by  the  filter  when  subjected 
to  a  "real  world"  environment.  Depicting  the  "real  world"  with 
a  truth  model  provides  an  analysis  tool  for  determining 
accurate  statistics  which  describe  the  filter's  errors.  Two 
techniques  are  available  to  meet  this  objective,  a  covariance 
analysis  and  a  Monte  Carlo  study.  However,  the  measurement 
model  in  this  study  is  nonlinear  and  nonlinear  effects  cannot 
be  fully  evaluated  by  a  covariance  analysis.  (Ref  7:p329)  This 
study  therefore  uses  a  Monte  Carlo  technique  for  evaluating 
filter  performance. 

For  this  study,  the  "real  world"  is  depicted  by  a  truth 
model  that  was  first  developed  by  Mercier  and  then  expanded 
by  Harnly  and  Jensen.  (Refs  13;  6)  As  indicative  of  the 
"real  world",  this  model  provides  for  (1)  realistic  missile 
dynamics,  (2)  a  full  implementation  of  the  third  order 
atmospheric  effects  as  defined  in  (2-5),  and  (3)  simulated 
target  FUR  and  background  noises  of  variable  and  realistic 
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spatial  and  temporal  correlation.  The  block  diagram  in  Fig 
7  depicts  the  structure  of  a  Monte  Carlo  study.  Numerically 
generated  white  Gaussian  noises,  w(t^)  and  v(t^)  ,  drive  the 
truth  model  which  produces  both  sample-by-sample  simulations, 
xft^),  and  noisy  measurements,  £(t^) ,  representative  of  the 
"real  world"  stochastic  processes.  The  tracking  filter  proc¬ 
esses  z_(t^)  and  outputs  the  state  estimate  x(t^)  .  In  line 
with  the  closed-loop  operation  of  a  pointing- tracking  system, 
the  filter  also  provides  pointing  commands  to  the  truth  model 
in  the  form  of  target  position  estimates.  The  simulated 
center  of  the  FLIR  field  of  view  is  then  pointed  to  the 
predicted  target  location  prior  to  the  next  sample  time. 

A  Monte  Carlo  study  keeps  track  of  the  filter  estimation 
errors,  eCtp,  over  many  runs  of  these  sample-by-sample  simu¬ 
lations.  The  statistics  of  these  errors,  means  and  standard 
deviations,  then  describe  the  filter’s  performance.  As  point¬ 
ed  out  by  Mercier,  the  trend  of  these  error  statistics  as 
the  number  of  simulation  runs  increases  indicates  how  well  the 
error  sample  statistics  represents  the  true  error  statistical 
characteristics.  In  fact,  he  found  that  twenty  simulation 
runs  were  sufficient  to  demonstrate  convergence  in  the 
statistics.  (Ref  13:p42)  This  was  shown  by  plotting  the 
standard  deviation  of  the  error  at  a  number  of  chosen  times 
versus  the  number  of  simulation  runs.  As  the  number  of 
simulation  runs  increased,  the  standard  deviation  converged 
to  a  steady  value.  This  same  method  was  used  to  demonstrate 
convergence  in  this  study's  more  dynamic  environment.  An 
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Figure  7 
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example  of  this  convergence  is  shown  in  Fig.  8. 

As  mentioned  in  Section  II,  off-line  tuning  is  re¬ 
quired  to  obtain  effective  filter  performance.  Tuning  consists 
of  adjusting  the  correlation  times  and/or  noise  strengths  in 
the  filter  model  until  Monte  Carlo  runs  indicate  effective 
performance.  This  study  performed  tuning  by  adjusting  the 
discrete  dynamic  driving  noise,  Qp^  ,  the  target's  maximum 
intensity,  Imax>  and  the  discrete  measurement  noise,  R ^ , 
within  the  filter  model.  Changing  these  values  causes  the 
filter's  estimation  error  statistics  to  vary.  In  this  study, 
effective  performance  is  achieved  by  minimizing  the  error  on 
all  filter  states  simultaneously.  Other  problems  may  dictate 
other  performance  indicators  such  as  minimizing  the  error  on 
only  specified  states. 

Agreement  between  the  filter's  estimate  of  the  variance 
of  the  error  with  the  actual  error  variance  for  each  state  is 
one  indication  of  proper  tuning.  When  these  values  closely 
match  over  the  time  period  of  the  simulation,  the  actual  error 
committed  by  the  filter  should  be  minimized.  If  the  filter  is 
underestimating  or  overestimating  the  true  variance,  the  filter 
gains  will  not  be  optimal,  and  larger  true  errors  result. 
Another  indicator  for  tuning  the  EKF  is  trying  to  match  the 
f i Iter -computed  variance  with  the  Monte  Carlo  correlation 
(variance  +  mean  ),  since  the  EKF  has  the  potential  for 
producing  biased  estimates. 

Four  target  trajectories,  each  describing  different 
air-to-air  missile  dynamics,  are  used  in  the  performance 
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analysis.  Defined  in  an  inertial  reference  frame  whose 
origin  was  located  at  the  tracker,  the  trajectory  equations 
are  summarized  in  Table  1.  For  a  detailed  development  of 
the  entire  truth  model  see  Appendix  A. 

The  first  trajectory,  a  constant  velocity  cross  range 
path,  was  primarily  used  during  software  verification  and 
robustness  studies.  Extended  Kalman  filter  performance 
for  this  trajectory  was  compared  to  a  similar  test  con¬ 
ducted  by  Harnly  and  Jensen  to  verify  correct  software 
implementation  of  the  baseline  EKF  and  the  entire  Monte 
Carlo  simulation.  Since  the  linear  dynamics  model  (2-19) 
is  well  suited  to  this  benign  trajectory  (Ref  6:pll4), 
nonlinear  measurement  effects  due  to  a  turning  missile's 
acceleration  are  eliminated.  Thus,  this  trajectory  is  appro¬ 
priate  for  evaluating  filter  robustness  to  other  mismodelling. 
Those  checked  in  this  study  were  imperfect  initial  conditions, 
incorrect  filter  target  intensity,  Imax>  and  signal- to-noise 
ratios  lower  than  assumed  in  the  filter. 

The  remaining  three  trajectories  are  constant  accel¬ 
eration  turns  of  5,  10,  and  20  g's.  These  are  intended 
to  be  representative  of  air-to-air  missile  maneuvering. 

Since  these  turns  are  not  well  modelled  by  the  filter, 
nonlinear  effects  appear  which  are  not  well  addressed  by  the 
EKF.  However,  it  is  specifically  with  these  maneuver  effects 
that  the  alternative  filters  may  demonstrate  better  per¬ 
formance  . 
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Table  1 


Truth  Model  Trajectories 


Constant  Velocity 

xe  =  -550.33  m/sec 
ye  =  -530.33  m/ sec 
ze  =  0  m/sec 


x  =  1000  m 
o 

y  =  1800  m 
o 

z  =  15,000  m 
o 


Constant  Acceleration  Turns 
Time  <  2  sec 

xe  =  -750  m/sec 
ye  =  0  m/sec 
ze  =  0  m/sec 
Time  >  2  sec 

xe  =  -750  cost  «(t-AT/2-2)  ] 
ye  =  750  s in[«( t -AT/2 -2 ) ] 


x  =  7000  m 
o 

y  =  500  m 

o 

z  =  15,000  m 
0 


ze  =  ° 

xe  =  5500  -Rs in  [co  ( t -  2  )  ] 
ye  =  500  +  R  (1  -cos  [o»(  t  -  2  )  ] } 
ze  =  15,000 


co ( rad/sec ) 

Rim  J 

5G 

0.0655776 

11,471.819 

10G 

0.1307553 

5735.9037 

20G 

0.2615106 

2867.9518 

t  =  current  time  in  simulation 


AT  =  sample  period 


Computational  Burden.  Analysis 

Designing  a  filter  algorithm  for  on-line  implementation 
often  involves  tradeoffs  between  filter  complexity  and  computer 
constraints.  Although  computer  memory  is  not  a  critical 
constraint  as  it  was  in  the  past,  the  number  of  real  time 
computations  required  is  still  a  valid  constraint,  especially 
in  systems  with  high  data  sample  rates.  With  this  in  mind, 
the  filters  in  this  study,  all  of  varying  complexity,  were 
analyzed  to  determine  their  computational  burden. 

In  this  study,  the  analysis  was  accomplished  by  simply 
counting  the  number  of  computations  required  to  complete  one 
filter  cycle,  i.e.  one  propagation  period  and  one  measurement 
update.  This  count  included  additions,  multiplications, 
exponential  evaluations,  and  number  of  inverses  required.  It 
must  be  noted  that  the  counts  in  this  study  function  only  as 
an  initial  estimate  of  the  computational  burden.  On-line 
implementable  filter  algorithms  utilize  additional  means  of 
reducing  complexity  such  as  exploiting  symmetric  or  sparse 
matrices,  transforming  to  canonical  state  space,  or  further 
model  simplification.  Also,  to  surmount  on-line  numerical 
difficulties,  square  root  filtering  techniques  may  be 
employed.  Often  these  reintroduce  some  computational  burden. 
(Ref  7:356,368) 
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V.  Results 


This  section  uses  a  comparitive  approach  in  analyzing  the 
alternative  filters'  performance  and  computational  burden.  Per¬ 
formance  results  from  the  extended  Kalman  filter  (EKF)  are  the 
baseline  for  this  study.  The  filters'  best  performance  establish¬ 
ed  under  the  assumed  ideal  conditions  is  presented  first.  To 
judge  their  performance  in  a  more  realistic  envirnment,  and  to 
look  at  areas  where  EKF  performance  degrades,  the  ideal  condi¬ 
tions  are  changed  one  at  a  time.  Three  areas  are  considered 
1)  filter  response  to  imperfect  initial  conditions,  2)  performance 
during  low  signal- to-noise  ratio  conditions,  and 3}  performance  when 
the  filter  incorrectly  models  the  maximum  target  intensity,  I 

max 

Problems  were  encountered  with  this  study's  implementation 
of  the  statistically  linearized  filter  which  resulted  in  the 
filter  being  unable  to  track  the  target.  These  problems  are 
addressed  following  the  performance  results.  The  final  part 
of  this  section  presents  the  results  of  the  computational 
analysis . 

Within  this  section,  the  performance  criterion  of 
maintaining  track  on  the  target  is  defined  as  the  filter  com¬ 
puted  position  remaining  within  three  times  the  target  size 
(3 a  )  of  the  true  position.  For  any  size  target,  this  criter- 

gi 

ion  means  that  the  true  image  is  close  enough  to  the  predicted 
image  so  that  the  residuals  produce  useful  information. 

For  cases  of  large  a  ,  the  3 a  boundary  may  well  be  off 

the  8-by-8  tracking  window.  Many  of  the  results  are  tables 
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of  the  average  standard  deviations  and  average  absolute  errors 
from  Monte  Carlo  simulations.  The  values  listed  are  time 
averages  over  the  five  second  duration  of  the  simulation  run. 
As  a  final  note  of  introduction,  the  individual  case  para¬ 
meters  and  performance  plots  are  contained  in  Appendix  E  for 
the  EKF,  Appendix  F  for  the  EKF  with  Bias  Correction  Term, 
and  Appendix  G  for  the  Constant  Gain  EKF. 

Best  Performance 

Looking  first  at  the  EKF  with  bias  correction  term,  the 
results  in  Tables  II  and  III  show  how  closely  it  matches  the 
performance  of  the  baseline  EKF.  In  fact,  the  additional 
second  order  bias  correction  term  did  not  succeed  in  reducing 
the  biases  which  occurred  during  the  turn  trajectories. 
Throughout  the  Monte  Carlo  study,  as  exemplified  by  the  re¬ 
presentative  sample  in  Table  IV,  the  bias  correction  term 
elements  were  consistently  an  order  of  magnitude  smaller  than 
the  filter's  predicted  measurement.  The  second  order  infor¬ 
mation  was  thus  deemed  insignificant  within  the  tracking 
filter,  which  resulted  in  no  appreciable  performance  improve¬ 
ment  . 

As  a  result  of  this  finding,  only  the  constant  gain 
EKF  underwent  a  performance  analysis,  and  not  the  associated 
constant  gain  EKF  with  bias  correction  term.  The  results 
for  this  filter  are  summarized  in  Table  II.  For  the  con¬ 
stant  velocity  trajectory  case,  performance  matched  the 
baseline  EKF  quite  well,  but  the  results  were  not  as  favor- 
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Table  II 


Performance  Comparison 
Ideal  Conditions 


Extended  Kalman 
Filter 

EKF  with  Bias 
Correction  Term 

Constant 
Gain  EKF 

Case:  Constant  Veloctity 

El 

FI 

Gl 

Avg  Standard  Deviation 

X 

y 

X 

y 

X 

y 

Position  (pixels) 

Actual 

0.17 

0.17 

0.17 

0.17 

0.17 

0.17 

Filter 

0.20 

0.20 

0.20 

0.20 

Velocity  (pixels/sec) 

Actual 

1.2 

1.2 

1.2 

1 . 2 

1.3 

1.2 

Filter 

1.8 

1.8 

1.8 

1.8 

Average  Absolute  Error 

Position  (pixels) 

0.04 

0.04 

0.04 

0.04 

0.04 

0.04 

Velocity  (pixels/sec) 

0.3 

0.3 

0.3 

0.3 

0.3 

0.3 

Case:  5G  Turn 

E2 

F2 

G2 

Avg  Standard  Deviation 

x 

y 

X 

y 

X 

y 

Position  (pixels) 

Actual 

0.19 

0.15 

0.18 

0.13 

0.29 

0.16 

Filter 

0.22 

0.19 

0.21 

0.19 

Velocity  (pixels/sec) 

Actua 1 

1.4 

1.2 

1.3 

1.1 

2.8 

1.3 

Filter 

2.1 

1.9 

1.9 

1.7 

Average  Absolute  Error 

Position  (pixels) 

0.05 

0.07 

0.07 

0.09 

0.14 

0.07 

Velocity  (pixels/sec) 

1.2 

2.3 

1.3 

2.6 

1.6 

2.4 

Table  III 


Performance  Comparison 


Id 

eal  Conditions 

Extended  Kalman 
Filter 

EKF  with  Bias 
Correction  Term 

Case:  10G  Turn 

E3 

F3 

Avg  Standard  Deviation 

x 

y 

x  y 

Position  (pixels) 

Actual 

0.19 

0.16 

0.18  0.16 

Filter 

0.21 

0.19 

0.21  0.19 

Velocity  (pixels/sec) 

Actual 

1.6 

1.3 

1.5  1.3 

Filter 

2.1 

1.9 

2.1  1.9 

Average  Absolute  Error 

Position  (pixels) 

0.08 

0.13 

0.08  0.13 

Velocity  (pixels/sec) 

1.2 

4.4 

2.3  4.7 

Case:  20G  Turn 

Avg  Standard  Deviation 

X 

E6 

y 

F6 

X 

y 

Position  (pixels) 

Actual 

0.20 

0.16 

0.20 

0.16 

Filter 

0.23 

0.20 

0.23 

0.20 

Velocity  (pixels/sec) 

Actual 

2.6 

2.4 

2.6 

2.4 

Filter 

4.5 

4.1 

4.5 

4.1 

Average  Absolute  Error 


Position  (pixels)  0.04  0.05 

Velocity  (pixels/sec)  Z.4  7.0 


0.04  0.05 

2.5  7.0 


on  Term  and  Filter  Measurement  Comparison 
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able  for  the  turn  trajectories.  The  constant  gain  matrix 
had  been  designed  to  work  for  both  the  5G  and  10G  turns  (see 
Appendix  C) .  Although  the  constant  gain  filter  maintained 
track  for  the  5G  turn,  its  performance  statistics  showed  a 
significant  performance  degradation  from  the  baseline  EKF, 
especially  in  the  x  direction.  This  same  filter  was  unable 
to  maintain  track  on  any  of  the  20  Monte  Carlo  10G  turn 
simulation  runs.  No  further  constant  gains  were  tried,  such 
as  using  a  workable  10G  turn  gain  during  the  5G  turn. 

Imperfect  Initial  Conditions 

Both  the  EKF  and  EKF  with  bias  correction  term  under¬ 
went  an  acquisition  sequence  for  this  portion  of  the  study. 
The  first  part  of  this  sequence  was  setting  the  initial  co- 
variance,  P  ( 0) ,  to  values  of  25  pixels2  for  the  dynamics 
position  states,  200  pixels2/sec2  for  the  velocity  states, 

400  pixels2 /sec4  for  the  acceleration  states,  and  0.2  pixels2 
for  the  atmospheric  jitter  states.  These  values  represent 
the  uncertainty  of  the  initial  conditions  given  the  filter. 
The  other  portion  of  the  acquisition  sequence  involved  a 
variable  Qp^  matrix  for  the  first  0.5  second  of  each 
simulation  run.  Values  within  Qp^  were  linearly  decreased 
to  25%  of  their  initial  value  during  this  acquisition 
time  period.  This  acquisition  sequence  causes  the  filter 
gain  to  stay  appropriately  high  for  the  0.5  second  normally 
required  for  acquisition,  and  was  also  used  by  Harnlv  and 
Jensen  (Ref  6:pl04).  However,  their  P(0)  matrix  (based  on 
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uncertainties  inherent  in  a  realistic  application  with  hand- 
off  from  a  radaT)  resulted  in  initial  transients  on  some 
simulations  of  this  study  which  resulted  in  the  filter  losing 
track.  The  previous  values  were 

25 

25  2000  2000 

P(0)=  100 

Thus,  the  values  cited  for  this  study  were  obtained  empiri¬ 
cally  to  get  good  acquisition  performance,  necessitating  an 
alteration  of  the  third  through  sixth  diagonal  entry. 

One  way  to  analyze  the  recovery  from  imperfect  initial 
conditions  is  to  compare  the  Monte  Carlo  mean  error  time 
history  for  the  first  eight  sample  times  as  is  done  in  Table 
V.  The  initial  conditions  for  the  results  in  this  table 
were 

/v 

xd(0)  =  0.5  pixels 

A 

yd(0)  =  0.5  pixels 
xe(0)  =  525  m/sec 
ye(0)  =  535  m/sec 
zg(0)  =  8  m/sec 

which  represent  a  1.5%  error  in  the  true  initial  conditions. 
Notice  that  the  EKF  with  bias  correction  term  has  a  larger 
position  mean  error  at  the  second  and  third  sample  times, 
although  it  does  reach  the  average  absolute  mean  error  of 
0.04  pixels  at  the  same  time  as  the  EKF.  This  larger  bias 
can  be  understood  by  comparing  the  bias  correction  term 
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Table  V 


Time 

0.033 

0.066 

0.100 

0.133 

0.166 

0.200 

0.233 

0.266 


0.033 

0.066 

0.100 

0.133 

0.166 

0.200 

0.233 

0.266 


Initial  Conditon  Mismatch  Recovery  Comparison 

X  Channel 

Dynamics  Position  Mean  Error 


Extended  Kalman 
Filter 

Constant 
Gain  EKF 

EKF  with  B 
Correction 

-0.223 

-0.543 

-0.223 

-0.293 

-0.536 

-0.378 

-0.247 

-0.555 

-0.329 

-0.137 

-0.515 

-0.180 

-0.082 

-0.428 

-0.100 

0.006 

-0.274 

-0.002 

0.019 

-0.155 

0.018 

0.033 

-0.054 

0.034 

X  Channel 

Velocity  Mean  Error 

-15.975 

-12.202 

-15.975 

-4.335 

-8.824 

-6.488 

-1.026 

-6.080 

-1.876 

1.152 

-3.352 

1.371 

1.601 

-0.738 

2.168 

2.339 

1.894 

2.911 

1.722 

3.704 

2.209 

1.256 

4.933 

1.617 

Constant  Velocity  Trajectory 
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elements  to  the  predicted  filter  measurement  during  the  ac¬ 
quisition  phase.  The  comparison  of  these  values  in  Table 
VI  shows  how  the  filter  takes  several  sample  periods  to  sort 
out  the  true  magnitudes  of  the  terms.  In  contrast,  the 
constant  gain  EKF  takes  much  longer  in  Table  V  to  reach  the 
0.04  pixels  mean  error  criterion.  This  is  expected,  since 
this  filter  cannot  take  advantage  of  the  acquisition  se¬ 
quence's  influence  on  P(t^)  which  helped  place  more 
emphasis  on  the  initial  measurements  within  the  baseline 
EKF. 

As  mentioned  before,  Tables  V  and  VI  correspond  to 
initial  state  values  that  constitute  a  1.5%  error  in  the  true 
initial  conditions.  Simulation  runs  were  also  made  for 
values  constituting  a  2.51  error.  The  results  of  these  runs 
(cases  E-7  and  E-8  in  Appendix  E  and  cases  F-7  and  F-8  in 
Appendix  F)  showed  that  neither  the  EKF  nor  EKF  with  bias 
correction  term  was  able  to  maintain  track  when  subjected 
to  just  the  errors  in  initial  velocity.  For  the  case  of 
just  initial  position  error,  the  EKF  maintained  track  in  all 
20  simulation  runs,  whereas  the  EKF  with  bias  correction  term 
lost  track  in  2  of  the  20  runs.  This  substantiates  the 
previous  degraded  performance  of  the  EKF  with  bias  cor¬ 
rection  term  in  Table  V.  It  also  demonstrates  that  both 
of  these  filters  are  more  sensitive  to  initial  velocity 
errors.  Due  to  time  constraints,  the  constant  gain  EKF  was 
not  tested  with  the  larger  initial  condition  errors. 
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Table  VI 


Low  Signal- to-noise  Ratio  Performance 

This  study's  signal-to-noise  ratio,  I  /cr,,,  is  an 
indicator  of  the  intensity  of  the  target  image  relative  to 
the  rms  background  and  FLIR  measurement  noise.  To  test  its 
effect  on  filter  performance,  the  signal-to-noise  ratio  was 
reduced  from  the  ideal  12.5  to  a  lower  value  of  2  by  both  de¬ 
creasing  Imax  and  increasing  (Realistic  signal-to-noise 

ratios  for  this  problem  are  between  1  and  25.)  The  results 
of  changing  this  operating  condition  are  summarized  in  Table 
VII  for  both  the  baseline  EKF  and  EKF  with  bias  correction 
term  tracking  the  constant  velocity  trajectory.  Both  fil¬ 
ters  again  demonstrate  identical  performance,  but  notice  how 
their  performance  significantly  degrades  for  the  Imax=4 
cases.  This  degradation  is  caused  by  the  shape  of  the  in¬ 
tensity  profile  changing  dramatically  for  Imax=4>  which 
makes  it  hard  for  the  filter  to  seek  out  good  measurement 
information  in  the  noisy  data.  The  constant  gain  EKF  was 
not  tested  for  this  set  of  conditions. 

Incorrect  Filter  Modelling  Performance 

This  situation  was  meant  to  test  filter  robustness 
(the  filter's  tracking  ability  when  some  portion  of  its 
internal  model  inaccurately  portrays  the  "real  world") . 

The  maximum  target  intensity  was  chosen  as  the  mismodelled 
parameter  and  the  performance  plots  in  Figs  9  and  10  help  to 
emphasize  the  filter  comparison.  These  figures  depict  the 
time  history  of  the  x^  mean  error  +  one  standard  deviation 
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Table  VII 


i 


Performance  Comparison 
Low  Signa 1 -to-Noise  Ratio,  S/N  =  2 


Extended  Kalman 
Filter 

EKF  with  Bias 
Correction  Term 

Case:  “max'4-  «n*2 

Avg  Standard  Deviation 

X 

Ell 

y 

FI  1 

X 

y 

Position  (pixels) 

Actual 

0.44 

0.44 

0.44 

0.44 

Filter 

0.38 

0.38 

0.38 

0.38 

Velocity  (pixels/sec) 

Actual 

3.9 

4.0 

3.9 

4.0 

Filter 

4.8 

4.8 

4.8 

4.8 

Average  Absolute  Error 

Position  (pixels) 

0.10 

0.12 

0.10 

0.  12 

Velocity  (pixels/sec) 

0.8 

1.0 

0.8 

1.0 

Case:  Im  =25, a  =12.5 
max  n 

Avg  Standard  Deviation 

X 

E12 

y 

FI  2 

X 

t 

y 

Position  (pixels) 

Actual 

0.19 

0.17 

0.19 

0.17 

Filter 

0.21 

0.21 

0.21 

0.21 

Velocity  (pixels/sec) 

Actual 

2.5 

2.4 

2.5 

2.4 

Filter 

3.7 

3.7 

3.7 

3.7 

Average  Absolute  Error 

Position  (pixels) 

0.03 

0.04 

0.03 

0.04 

Velocity  (pixels/sec) 

0.5 

0.5 

0.5 

0.5 

for  the  20  Monte  Carlo  simulation  runs.  Fig  9  depicts  the 
divergent  performance  of  the  EKF  with  Imax  mismodelled  as 
twice  its  truth  model  value.  The  two  peaks  after  t=4  seconds 
represent  loss  of  track  in  two  of  the  runs  (in  computing 
statistics,  error  values  are  set  to  zero  after  loss  of 
track  occurs) .  Identical  performance  was  recorded  for  the 
EKF  with  bias  correction  term.  Fig  10  depicts  the  improved 
performance  of  the  constant  gain  EKF.  True  to  the 
claims  in  Section  III,  this  filter  is  more  robust  to  filter 
mismodelling  of  Im  .  This  results  from  the  constant  gain 
not  being  a  function  of  the  covariance  calculations  which 
contain  the  mismodelled  filter  approximation  through  the 
partial  derivatives  in  HCt^) . 

Statistically  Linearized  Filter 

The  statistically  linearized  filter  developed  in  Sec¬ 
tion  III  was  able  to  maintain  track  during  the  constant  vel¬ 
ocity  trajectory  with  ideal  conditions  for  only  1.1  seconds. 
This  unacceptable  performance  is  primarily  attributed  to  the 
particular  discretization  used  in  this  study.  Particularly 
for  the  constant  velocity  case,  the  conditional  probability 
density  function,  f  i  ,  is  tightly  packed  about  the  mean 

\  j 

x(t^  ).  Thus  the  values  of  in  the  eight  pixels  surrounding 

/V  A 

the  pixel  containing  ^peak(ti) ,ypeak(ti) ]  approach  zero. 

When  this  occurs,  the  matrix  H(t^)  approaches  £  because  of 
the  filter  discretization.  Rewriting  the  expression  (3-19) 
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Figure  10 
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W(t.)  =  (h[x(ti)  ,ti]xT(ti)  -  h[x(t.),ti]xT(ti')}  P'l(ti‘) 

note  that  when  the  value  of  P £ for  the  center  pixel  equals  one 
(due  to  small  rms  tracking  error  relative  to  pixel  size) , 

(3-19)  changes  to 

«(tA)  =  {h[x(ti'),ti]xT(ti')  -  h[x(ti‘),ti]£T(ti’)}. 
P'Cti")  =  0 

A  better  approximation  for  determining  H(t^)  should  result 

in  the  convergence  of  H(t.)  to  H(t^)  for  this  case. 

To  verify  this  condition,  several  simulation  runs  were 

performed  using  arbitrary  values  for  the  nine  element 

discretization  of  f  i  within  the  filter.  Table  VIII 

x  |  z 

summarizes  the  performance  of  these  runs  by  comparing  filter 
state  estimates  to  those  of  the  EKF  and  also  with  the  truth 
model  values.  These  values  are  taken  at  a  single  time  point 
from  a  single  simulation  sample,  and  they  are  indicative  of 
the  problem's  nature.  For  larger  values  of  P  ,  for  the 
pixel  containing  x(t^)  ,  the  performance  degrades  substantially, 
especially  in  estimating  the  acceleration.  The  associated 
H_(t^)  matrix  elements  also  decreased  in  magnitude  with 
larger  values  of  P  . 

When  the  filter  was  allowed  to  compute  its  own  P^ 
values,  simulation  results  showed  that  initially  the  P^ 
values  were  essentially  equal,  as  dictated  by  the  large 
initial  covariance  P(Q)  described  earlier.  Subsequently, 

A  _ 

fx| z  collapsed  about  x(t^  )  until  the  tenth  sample  period 
when  P  reached  a  maximum  of  0.94.  At  this  point  the 
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Va lues  at  t \ =1  . 1  see 


discretization  problem  effects  took  hold  and  the  filter's 
response  was  a  spreading  out  of  f  t  .  Within  another  ten 

X  |  u 

sample  periods  had  reached  a  steady  state  value  of  0.26. 
Computational  Burden 

The  results  of  this  analysis  are  listed  in  Table  IX, 
indicating  the  number  of  operations  required  for  a  single 
sample  period.  Compared  to  the  baseline  EKF,  the  EKF  with 
bias  correction  term  requires  four  times  as  many  computations, 
whereas  the  constant  gain  EKF  requires  only  one-tenth  the 
computations.  This  study's  implementation  of  the  statisti¬ 
cally  linearized  filter  required  twice  as  many  computations 
as  the  baseline  EKF,  and  this  count  is  very  much  a  function 
of  the  means  chosen  to  approximate  the  evaluation  of  the 
conditional  expectations  inherent  in  the  filter's  structure. 

As  mentioned  in  Section  IV,  these  results  should  only 
be  used  as  an  estimate  of  what  actual  on-line  computational 
requirements  might  be. 


State  Update  576  512 

1144  1472 


Compute  Bias  Correction  Term  29,504  .35,520 

30,712  36,992 


VI.  Conclusions  and  Recommendations 


Conclusions 

The  scope  of  this  study  dictates  making  conclusions 
about  each  of  the  four  alternative  filters  based  on  the  re¬ 
sults  just  presented.  These  conclusions  are  based  on  com¬ 
parisons  with  the  baseline  extended  Kalman  Filter. 

Extended  Kalman  Filter  with  Bias  Correction  Term.  The 
EKF  with  bias  correction  term  and  its  associated  constant 
gain  formulation  do  not  present  any  noticeable  performance 
improvement.  Especially  when  considering  the  added  com¬ 
putational  burden  of  the  bias  correction  term,  these  two 
filters  are  not  worth  implementing  for  on-line  application. 

Constant  Gain  Extended  Kalman  Filter.  As  predicted, 
the  constant  gain  EKF  displayed  superior  robustness,  but 
overall  performance  was  tied  directly  to  the  quality  of  the 
gain  selection.  In  this  study,  performance  while  tracking 
maneuvering  targets  was  not  as  good  as  the  baseline  EKF. 

The  small  computational  loading  is  impressive  and  inviting 
for  on-line  implementation.  Finally,  a  constant  gain  filter 
may  require  additional  help  in  the  acquisition  phase  due  to 
its  poorer  incorrect  initial  condition  recovery  performance. 

Statistically  Linearized  Filter.  As  implemented  in  this 
study,  the  statistically  linearized  filter  was  unsuccessful. 
This  was  due  to  the  discretization  used  approximating  the 
conditional  probability  density  function,  f^i  .  This  filter 
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was  developed  to  gain  improved  performance  when  rms  tracking 
errors  for  the  EKF  exceed  one  pixel.  However,  this  level  of 
error  was  never  induced  for  long  periods  of  time  during  the 
cases  in  this  study.  As  a  result,  the  particular  discretiz¬ 
ation  used  would  have  heloped  little  to  enhance  performance 
even  if  it  did  not  suffer  from  other  problems. 

Recommendations 

Further  research  into  these  alternative  filters  should 
center  about  the  constant  gain  extended  Kalman  filter  and 
statistically  linearized  filter. 

The  small  computational  burden  of  the  constant  gain 
EKF  is  a  desirable  on-line  attribute  that  could  be  exploited 
by  looking  for  an  "all  purpose"  gain.  Perhaps  defined  by 
pole  placement  or  entire  eigenstructure  assignment  techniques, 
such  a  gain  would  be  successful  at  tracking  a  maneuvering 
target  not  constrained  to  a  specified  trajectory.  In  light 
of  this  filter's  poor  acquisition  performance,  an  on-line 
implementation  might  employ  an  extended  Kalman  filter  com¬ 
puting  its  own  gain  for  some  specified  acquisition  phase. 
Another  possible  approach  would  be  a  scheduled  gain  structure. 
During  acquisition  or  harsh  maneuvers  (detected  by  residual 
monitoring)  a  higher  constant  gain  could  b-  u  -^.with  the 
lower  gain  used  for  normal  tracking. 

The  statistically  linearized  filter  could  be  revamped 
by  defining  a  new  discretization  scheme.  This  might  include 
a  variable  scheme  that  switches  from  a  one  pixel  defining 
area  of  f>2  to  the  nine  pixel  area  used  in  this  study 
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depending  upon  the  values  of  P(t^  )•  Regardless,  the  dis¬ 
cretization  should  be  smaller  than  one  pixel  to  avoid  the 
problem  encountered  in  this  study.  Such  a  finer  discretiz¬ 
ation  however,  must  be  weighed  against  the  additional  com¬ 
putational  burden.  It  would  be  desirable  to  find  an 
approximation  such  that  tf(t^)  approximates  H(t^) ,  instead 
of  when  the  probability  that  the  true  target  intensity 
center  lies  within  a  single  discrete  area  goes  to  one. 

Other  alternatives  for  evaluations  of  the  inherent  condi¬ 
tional  expectations  should  also  be  explored.  There  are 
several  scenarios,  such  as  targets  performing  jink  man¬ 
euvers,  that  develop  large  enough  P(t.~)  values  where  this 
filter  migh  outperform  the  EKF. 
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Appendix  A 


Baseline  Extended  Kalman  Filter  and  Truth  Model 

This  appendix  presents  the  additional  equations, 
matrices,  and  other  information  used  to  construct  the 
baseline  EKF  and  truth  model.  The  approach  in  this  section 
is  to  present  only  enough  detail  to  reconstruct  the 
algorithms  in  this  study.  For  the  detailed  development,  the 
reader  is  asked  to  refer  to  the  work  of  Mercier  (Ref  13)  and 
Harnly  and  Jensen  (Ref  6). 
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7  8 

where  o^2  is  the  strength  of  the  acceleration  noise  and 
2  is  the  variance  of  the  atmospheric  jitter  model  out¬ 
put.  As  noted  in  Section  II  the  covariance  of  the  measure¬ 
ment  model  noise,  R,  is  computed  as  (1/RpH.  Adaptive 
size  and  shape  estimation  is  accomplished  through  the 


equations 


ARp(t  J  -  ARF(ti_1)  +  c  3AR  |  =  pj? 


(A- 3) 


a  _  (t.)  =  a  _(t.  .)  +  c  '  A  (A-4) 

g,F  i'  g.F1-  i-1'  J0gi  j  ogt-  0gif 

where  ARp  is  defined  as  o p/ag^p,  c  is  a  constant 

determined  empirically,  and  L  is  the  quadratic  cost  function 

defined  as 


L(x(ti),  ogi,  AR,  0,  ti)  = 

CiCtiJ-hCxCtp"),  ti)]T  [z(ti)-h(x(ti'),  tp)] 
The  angle  6  is  shown  later  in  Fig  A-2. 


(A- 5) 
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Truth  Model 

Tra  jector ies  .  In  developing  the  trajectories 
presented  in  Table  I,  a  nominal  velocity  of  750  meters/ 
second  was  chosen,  which  corresponds  to  a  Mach  number  of 
2.26  at  standard  temperature  and  pressure.  These  sample 
flight  paths  are  all  defined  in  the  coordinate  frame  shown 
in  Fig  A-l. 

The  target's  range  from  the  tracker  is 

R  =  Vxe2  +  V  +  ze2  [A'5) 

and  it's  associated  velocity  is 

V  =  ^xQz  +  yg2  +  zg2  (A-6) 

Two  more  defining  relationships  which  govern  a  constant 
accleration  turn  are 

a  =  oj2  r  (  A-  7  ) 

V  =  a)  r  ( A  -  8  ) 


where  a  is  the  acceleration,  w  the  angular  velocity,  r  the 
turn's  radius,  and  V  the  same  velocity  as  in  (A-6).  The 
trajectory  equations  were  inserted  into  the  Monte  Carlo 
study  through  subroutine  TRAJEC  (see  Appendix  DJ . 

For  the  constant  velocity  trajectory,  the  total 
velocity  is  split  evenly  between  the  x  ana  y  directions. 
From  equation  (A-6) 

7so  =  Vy  *  y 

|xe|  =  550.33  m/sec 

|y  !  =  530.35  m/sec 


The  trajectory  path  is  at  a  constant  altitude  and  brings  the 
target  close  by  the  tracker  to  induce  an  inertial  acceler¬ 
ation  within  the  tracker  filter's  dynamics  model.  This  path 


is  described  by 

x  (t)  =  x  (0)  -  530 . 33( t ) 
e  e 

v  ( t )  =  v  (0)  -  530 . 5  3 ( 1 3 
e  e 

2  (  t  )  =  2  CO) 
e  e 

With  initial  conditions  of 


C  A- 9a  J 
( A-  9b ) 


l A- 9c  j 


xe(0)  =  1000  m 
ye CO)  =  1800  m 
2e(0)  =  15,000  m 

the  maximum  range  is  15,140  meters  and  the  minimum  range  is 
15,010  meters  during  a  five  second  run. 

Again,  the  constant  acceleration  turn  trajectory  is 
constrained  to  the  x-y  plane  at  an  altitude  of  15,000  meters 
also.  The  target  begins  the  run  moving  toward  the  tracker 
in  the  negative  x  direction.  At  time  t  ^  =  2  seconds,  the 
target  begins  the  turn  towards  the  positive  y  direction. 

For  this  constant  acceleration  turn,  the  velocity  components 
during  the  turn  are 


x  C  t  J  =  V  c  o  s  [  w  ( t  -  2  )  ] 
e  2 

y_(t)  =  V  s  i  n  [  co  ( t  -  2  )  1 
e  2 

ze(t)  =  0 


(  A  - 1  0a  J 


CA-lOb) 


(A- 10c  J 


where  is  the  velocity  magnitude  prior  to  starting  the 
turn.  The  trajectory  path  during  the  turn  is  described  by 


r  sin  [ cu ( t  -  2  J  ] 


( A- 1  la  ) 
U-llb) 
CA-llc J 


xe(t)  =  x  e(  2  )  - 

ye(t)  =  y  e(  2  )  -  r  (1  -  cos  [w(  t  -  2  J  ] } 
ze(t)  *  ze(2) 

where  the  parameters  co  and  r  are  evaluated  using  equation 
(A-7)  and  ( A - 8 )  at  the  desired  acceleration  level.  An  addi¬ 
tional  consideration  is  the  sample  data  nature  of  this  study. 
As  a  result,  the  truth  model  velocity  at  any  sample  time, 
t^,  should  be  representative  of  the  average  velocity  over 
the  previous  sample  period.  For  this  study,  the  average 
velocity  is  assumed  to  be  the  velocity  at  the  time  half  way 
between  sample  times.  Equations  (A-lOaJ  and  (A-lOb)  are 
then  adjusted  to 

xe(t )  =  V2c  os  r  to  ( t  -  AT  /  2  -  2  )  ]  (  A  - 1  2a  1 

ydt)  =  V  sin  (o»(  t-AT/2-2)  |  U-12b) 

where  AT  is  the  sample  period. 

Transformation  to  FLIR  image  plane.  Producing  the 
missile  image  on  the  FLIR  image  plane  from  the  position  and 
velocity  information  defined  in  the  inertial  reference  i  rame 
is  accomplished  by  Subroutine  XFORM  (.see  Appendix  DJ.  The 
basic  image  orientation  is  shown  in  Fig  A-J,  where  the  x 
direction  is  defined  as  azimuth,  a,  and  the  y  direction  as 
elevation,  3.  The  transformation  from  the  tracker  centered 
inertial  frame  to  FLIR  azimuth-elevation  elements  is 
accomplished  by  the  equations 


Y  ( e  la  voti  on) 


Intensity  Image  Orientation 
Figure  A  -  2 
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(A- 14) 


F - 


rh(t)ye(t)  ~  yeCt){[xe(t) + 

Y-xe(t)2  +  ye(t) 2  +  ze(t)2 
2e(t)ze(t)]/rh(t)} 


where  =  [ x  e  C  t ) 2  +  zQ(t)2]^2  .  The  truth  model  uses  a 

pixel  as  the  unit  of  angular  displacement  rather  that 
radians  (1  pixels  =  20  yrads).  Therefore,  a(t)  ana  8(t)  are 
converted  to  pixels/second  by  dividing  by  20  x  10~6  rads/ 
pixel. 


Truth  Model  Propagation.  The  complete  truth  model  time 
propagation  is  defined  by 


0 


adcti3  + 


wA(ti) 


CA- 15) 


LcVQAd 

where  the  state  vector  consists  of  two  dynamic  states  and 


six  atmospheric  jitter  states  arrayed  as 


-  ^XDT  yDT  XAT i  XAT2  XAT3  yAT j  yAT2  yAT 3 ^  (A-16) 


the  state  transition  matrix 


with  a  and  b  the  parameters  of  the  thiru  order  atmospheric 
jitter  model  described  in  Section  II  by  equation  (2-5), 


4  Ct -  +  At/2) 
0(t  l  +  At/2) 


(A- 18) 


5d(ti) 


At 

0 

0 

0 

0 

0 

0 
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0 

At 

0 

0 

0 

0 

0 

0 


(A- 19) 


and  wit)  is  a  vector  of  white  Gaussian  noises  each  witn  unit 
variance.  The  discrete  atmospheric  noise  strength,  > 
has  been  decomposed  with  a  Cholesky  square  root  algorithm 
(Ref  7  : p 3 7 0 )  to  form  , .  The  entire  discrete  noise 


strength  matrix,  developed  in  Appendix  D  of  Ref  6 

using  exact  integration  is  implemented  in  subroutine  TRUTHO 
(see  Appendix  D) . 

Truth  Model  Measurement.  The  truth  model  produces 
measurements  using  equaion  (2-6).  The  integral  is 
approximated  by  evaluating  the  function  (2-16)  at  the 
midpoints  of  sixteen  equal  subdivisions  of  each  pixel  and 
averaging  the  results.  These  measurements  are  then 
corrupted  by  a  64  dimensional  vector  of  noises,  v(t-), 
which  has  an  associated  noise  variance  matrix,  R,  defined  as 
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,64  (A-  20) 
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•  i 

r 

r 
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1 
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1 

i 

1 

1  ,  64 

2,64 

3  ,  64 

J 

where  the  coefficients  r  indicated  the  correlation 

m ,  n 

between  noise  componenets  v  and  v  at  a  given  time 

m  n 

t^.  Although  this  model  allows  tor  spatially  correlated 
noise  (Ref  6:pl7-21),  this  stuav  used  a  simple  diagonal  R 
matrix  (all  the  noise  components  are  uncorrelated)  by 
setting  the  FORTRAN  parameter  ISPTL  =  'NO' .  the  final  truth 
model  measurement  then  become s 

Ht^  =  hCxft^,^)  +  w(  t^  (A-  2  1 ) 

where  w(t^)  is  again  a  vector  of  independent  white 
Gaussian  noises  each  with  unit  variances. 
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Closed  Loop.  The  truth  model  incorportates  the 
filter's  position  estimates  with  the  following  equation 


xpeakT^  XDT  +  XAT  +  XAT  ’  xdF  (A-22a) 

1  1  2 

’'peak/*1  =  ^DT  *  ^AT  *  ^AT  ’  ^dF  <A-22b1 

1  1  2 
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Appendix  B 


Bias  Correction  Terras  -  Partial  Derivatives 


Computing  the  bias  correction  terms  in  Section  III 
requires  evaluating  the  matrix  of  second  partial  derivatives 

/V 

32h/3x-3xv,  with  the  function  h[x(t.)>  t.]  defined  as 

J  K  ——'ll 


h[x(ti),ti]  =  Imaxexp(-JsC(- 


(x-x_)cos6  +  (y-y _)  sine 


(B-l) 


(y-yp)cos0 - Cx-xp) Sine 


-)2]} 


where 


xpeak  =  xa  +  xd 


'  P 
°g 


^peak  ^a  +  ^d 

=  standard  deviation  in  direction  of  velocity  vector 


1 

o  =  standard  deviation  perpendicular  to  velocity 
vector 

8  =  angle  which  locates  the  velocity  vector  on  the 

FLIR  x-y  coordinate  frame  (see  Appendix  A). 

Notice  that  h[x(t^  ),t^]  is  only  a  function  of  four  states, 

and  that  derivatives  with  respect  to  x  and  x,  are  identical 

(results  from  Xp  =  xa  +  x^)  as  are  those  for  ya  and  y^. 

With  this  in  mind  the  partial  derivatives  follow. 

3  h 


3x. 


(PARTj)  ImaxexP^ARG^ 


(B-2) 
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where 


ARG  =  {-*[( 


(x-x^jcosQ  +  Cy-yp)sine  2 


)  + 


(y-yn)cose  -  (x-x  )sine  2 

( — 2 — - 2 - )  ]} 
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1  G  J 
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where 


(x-x  )cos0  +  (y-y  )sin0  sin0 
PART  =  {[ - 2 - - - E - ]  [ - ]  + 


(B-4) 


(y-yjcosa  -  (x-x_)sin.0  COS0 

r - E - - - E - ]  [- - ]} 


g. 


g 


3  2  h 


-cos20  sin2e 


3VXP 


=  r  r. 


]  +  (PART_)2}Imaxexp(ARG)  (B-5) 


3  2  h  -  sin2  0  cos2  0 


3V^P 


=  {[- 


-]  +  (PARTJ2}Imaxexp(ARG) 


3  2  h 


3xJyn 
P  P 


-sin0cos0  cos0sin0 


g 

2 


-]  +  (PART  )  (PART  )  )  •  (B-6) 

1  2 


I  exp (ARG) 
max  r  v  1 


34 


^AD-felU  1M 

1  UNCLASSIFIED 

AIR  FORCE  INST  OF  TECH  NRI8HT-PATTERSON  AFB  OH  SCHOO— ETC  F/8  17/S 
ALTERNATIVES  TO  AN  EXTENDED  KALMAN  FILTER  FOR  TARBET  IMABC  TRAC— ETC  (U) 

DEC  81  PR  LEUTHAUSCR 

AFIT/8A/AA/B1D-8  NL 

' 

a.3  mm 

i  ■  m 

— 

' 

r 

— 

II 

1 

1 

It 

i 

.   J 

lr 

■ _ 

r 


Appendix  C 


35 


Appendix  C 


Selection  of  a  Constant  Gain 
The  technique  chosen  to  establish  Kss  for  the 
turn  trajectories  was  the  linear  combination  of  the  gains 
obtained  in  Monte  Carlo  simulations  defined  by 

K  =  0.7  K  (t .=3.4)+0.3K  (t.=3.4)  (C-lJ 

-ss  -s  1  -10  i 

where 

K  (t.)  =  5  G  turn  gain  time  history 
“s  1 

K  (t-)  =  10G  turn  gain  time  history 

—i  o  1 

For  the  constant  velocity  trajectory,  the  gain  chosen  for 
-ss  was  £*2.633) ,  the  position  in  the  trajectory 
which  is  closest  to  the  tracker.  These  two  constant  gain 
matrices  are  listed  in  Tables  C-I  and  C - 1 1 . 
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Table  C-I 


Turn  Trajectory 


•373S-3 
.1052-2 
.1472-2 
.8452-3 
-.1392-3 
-.3902-3 
-.1792-3 
-.3752-4 
.3172-3 
.9 472-3 
•144E-2 

.856E-3 

-.244E-3 

-.5442-3 

-.2632-3 

-.6022-4 

•2452-3 

.3092-3 

.1325-2 

.8072-3 

-.3752-3 

-.748E-3 

-.3822-3 

-.9302-4 

.1812-3 

.6542-3 

.1152-2 

.7072-3 

-.5162-3 

-.957E-3 

-.5252-3 

-.1382-3 

.1272-3 

•5012-3 

.9552-3 

.5~2E-3 

-.6472-3 

-.1162-2 

-.6352-3 

-.1052-3 

.8552-4 

•  3652-3 
•744E-3 
.426E-3 
•746E-3 
-.1332-2 
-.842E-3 
-.2462-3 
•5522-4 
.2542-3 
.5492-3 
.2892-3 
-.796E-3 
-.144E-2 
-.4822-3 
-.3492-3 
.3422-4 
.1692-3 
. 368E-3 
.1752-3 
.7392-3 
.1462-2 
-.1092-2 
-.4122-3 


-.5192-3 

-.1942-2 

-.429E-2 

-.5562-2 

-.4192-2 

-.183E-2 

-.4612-3 

-.6712-4 

-.305E-3 

-.127E-2 

-.3132-2 

-.466E-2 

-.368E-2 

-.1762-2 

-.4842-3 

-.766E-4 

-.1392-3 

-.656E-3 

-.1792-2 

-.265E-2 

-.260E-2 

-.136E-2 

-.415E-3 

-.726E-4 

-.264E-4 

-.153E-3 

-.5022-3 

-.9372-3 

-.966E-3 

-.608E-3 

-.2122-3 

-.4252-4 

.3772-4 

.1932-3 

.5672-3 

.9532-3 

.9162-3 

.5012-3 

.156E-3 

.  2',4E— 5 
.6512-4 
.3822-3 
.1242-2 
,2522-2 
.2832-2 
.1832-2 
.683E-3 
.1492-3 
.683E-4 
.4462-3 
.1661-2 
.3592-2 
.446E-2 
•321E-2 
.134E-2 
•3362-3 
.6002-4 
.4242*3 
.1732-2 
•406E-2 
.5542-2  • 

.4382-2  • 
.2032-2  • 
.5572-3 


.4362-1 
.120 
.174 
•  111 

-.8002-4 

-.3542-1 

-.1762-1 

-.3792-2 

.3532-1 

.107 

.166 

.103 

-.  130E-I 

-.5322-1 
-.2712-1 
-.6192-2 
.2692-1 
.3912-1 
.149 
•969E-1 
-.3062-1 
-.758E-1 
-.3972-1 
-.9672-2 
..195E-1 
.708E-1 
.126 
.7932-1 
-.5172-1 
-.100 
-.556E-1 
-.146E-1 
•135E-1 
•531E-1 
.100 
.580E-1 
-.728E-1 
-.126 
-.7392-1 
-.2102-1 
.895E-2 
. 378E-1 
.7522-1 
.366E-1 
-.9022-1 
-.150 
-.929E-1 
-.289E-2 
.5672-2 
.2572-1 
.5312-1 
.1822-1 
-.102 
-.166 
-.111 
-.3772-1 
.346E-2 
.16?E-1 
.3542-1 
.446E-2 
-.104 
-.173 
-.124 
.465E-1 


-.6802-1 

.  101E-1 

-.1682-1 

-.254 

.2302-1 

-.6242-1 

-.559 

.41  IE-1 

-.137 

-.722 

•272E-1 

-.176 

-.542 

.150E-2 

-.133 

-.235 

-.737E-2 

-.5772-1 

-.5932-1 

-.3832-2 

-.1462-1 

-.4052-1 

-.168 

-.409 

-.580 

-.476 

-.225 

-.6172-1 

-.9672-2 

-.133E-1 

-.8752-1 

-.236 

-.370 

-.334 

-.174 

-.5232-1 

-.904E-2 

-.409E-2 

-.221E-1 

-.69OE-I 

-.123 

-.122 

-.7492-1 

-.254E-1 

-.4992-2 

.4612-2 

.2312-1 

.6972-1 

.120 

.120 

.6912-1 

.226E-1 

.4272-2 

.8112-2 

.492E-1 

.165 

.324 

.368 

.242 

.9192-1 

.2032-1 

.8712-2 

•  56ii- 1 

•  213 
.4o3 

•  579 
.419 
.176 
.4362-1 
.766E-2 
,5642-2 
.222 
.526 
.719 
.571 
.266 


-.837E-3 
.8142-2 
.2475-1 
.3902-1 
.2622-1 
-.1572-2 
-.113E-1 
-.5962-2 
-.137E-2 
.6132-2 
. 205E-1 
•34oE-l 
.2312-1 
-.6012-2 
-.165E-1 
- . 886E-2 
-.2172-2 
.4452-2 
.1612-1 
.2832-1 
.183E-1 
-.113E-1 
-. 226E-1 
-.1252-1 
-.330E-2 
■  305E-2 
.119E-1 
.225E-1 
.1272-1 
-.168E-1 
-.289S-1 
-.1682-1 
-.4802-2 
.2002-2 
.8452-2 
.165E-1 
.735E-2 
-.2182-1 
-.3472-1 
-.213E-1 
-.664E-2 
.1262-2 
.5672-2 
.1142-1 
.2772-2 
-.2472-1 
-.390E-1 
-.2562-1 
-.8692-2 
.764E-2 
•  363E-2 
.7412-2 
-•506E-3 
-.2622-1 
-.4112-1 
-.2902-1 


-.2092-2 

.1002-1 

-.4152-1 

-.100 

-.142 

-.116 

-.5522-1 

-.150E-1 

-.2362-2 

-.4702-2 

-.2172-1 

-.552E-1 

-.9102-1 

-.3172-1 

-.425E-1 
-.1272-1 
-.218E-2 
-.106E-2 
-.5642-2 
-.1752-1 
-.3042-1 
-.309E-1 
-.1802-1 
-.6072-2 
-.1172-2 
.103E-2 
•548E-2 
.1682-1 
.294E-1 
.297E-1 
.161E-1 
. 578E-2 
.1112-2 
.195E-2 
.1162-1 
.401E-1 
•795E-1 
.9072-1 
.5972-1 
.2282-1 
.5062-2 
.212E-2 
.1382-1 
•5Z2E-1 
.113 


.1172-2 
• 3252-2 
.4882-2 
•3352-2 
•3722-3 
-.7522-3 
-.4162-3 
-.9182-6 
•9402-3 
•287E-2 
.4532-2 
.3192-2 
-.3702-5 
-.1212-2 
-.654E-3 
-.152E-3 
. 708E-3 
.236E-2 
•3952-2 
.2752-2 
-.5532-3 
-.1812-2 
-.982E-3 
- . 24"E-3 
•506E-3 
•183E-2 
•3302-2 
.212E-2 
-.1242-2 
-.253E-2 
-.141E-2 
-0732-3 
. 345E-3 
•1352-2 
.2522-2 
.1402-2 
-.1962-2 
-.3312-2 
-.1922-2 
-.5462-3 

.2242-3 

.9382-3 
.1322-2 
.7142-3 
-.258E-2 
-.40 32-2 
-.246S-2 
-."6)2-3 
.1402-3 
.0232-3 
.1222-2 


-.2062-2 

-.7632-2 

-.1682-1 

-.2172-1 

-.1632-1 

-.7072-2 

-.1772-2 

-.2552-3 
-.1242-2 
-.5UE-2 
-.1242-1 
-.174E-1 
-.1422-1 
-.675E-2 
-.183E-2 
-.2872-3 
-.5852-3 
-.266E-2 
-.7202-2 
-.1112-1 
-. 998E-2 
-.518E-2 
-.153 
-.264E-3 
-.1372-3 
-.717E-3 
-.216E-2 
-.373E-2 
-.376E-2 
-.217E-2 
-.7232-3 
-.1382-3 
. 1 22Z-3 
•653E-3 
.2012-2 
• 359E-2 
.3672-2 
.2162-2 
.7342-3 
•1432-3 
.2362-3 
.1422-2 
.489E-2 
.  956E-2 
•111E-1 
."372-2 
.2822-2 
.6292-3 
.2532-3 

•  2  O 

.6382-2 


.7322-1  -.1032-1 


.142 

-. 302E-2 

•  *  J  —  -  4 

.1742-1 

.107 

-.458E-2 

.1272-1 

.4362-1 

-.293E-2 

.  'l'E-2 

.1082-2 

-.1012-2 

■  '-33E-2 

.1862-2 

.837E-4 

.2272-3 

.3'1?2-3 

.1632-2 

.5452-1 

.7615-3 

.6672-2 

.128 

.2482-3 

.1532-1 

.176 

-.319E-2 

.2162-1 

.140 

-.4362-2 

.1722-1 

.655E-1 

-•  339E-2 

.8032-1 

.  1302-1 

-.1252-2 

. 222E-2 
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Table  C-I  I 


Constant  \elocity  Turn 


Trajectory 


KT 

ss 


0.0 
0.0 
0.0 
0.0 
0.0 
-.001 
-.007 
-.004 
0.0 
0.0 
0.0 
0.0 
0.0 
-.00? 
-.004 
-.004 
0.0 
0.0 
0.0 
0.0 
-.0008 
-.003 
-.00  3 
-.002 
0.0 
.0007 
.001 
.0007 
-.001 
-.002 
-.002 
-.001 
.001 
.002 
.002 
.0009 
-.0007 
-.001 
-.0008 
0.0 
.002 
.003 
.003 
.0008 
0.0 
0.0 
0.0 
0.0 
.004 
.004 
.002 
.0005 
0.0 
0.0 
0.0 
0.0 
.004 
.003 
.001 


0.0 

0.0 

0.0 

0.0 

-.001 

-.002 

-.004 

-.004 

0.0 

0.0 

0.0 

-.0007 

-,o.t 

-.003 

-.004 

-.003 

0.0 

0.0 

0.0 

-.001 

-.002 

-.003 

-.003 

-.001 

0.0 

0.0 

0.0 

-.0007 
-.001 
-.0009 
-.0005 
0.0 
0.0 
0.0 
.0008 
.0009 
.0007 
0.0 
0.0 
0.0 
.001 
.002 
.003 
.002 
.002 
o.o 
0.0 
0.0 
.003 
.004 
.003 
.002 
.0007 
0.0 


0.0 

0.0 

0.0 

0.0 

0.0 


0.0 

0.0 

.004 

.004 

.002 

.001 

0.0 

0.0 

0.0 

0.0 


0.0 

0.0 

.001 

o.o 

-.02 

-.09 

-.2 

-.3 

0.0 

-.002 

.00? 

.003 

-.05 

-.2 

-O 

“•3 

.003 

.01 

.03 

.02 

-.0? 

-.2 

-•3 

-.2 

.02 

.05 

.08 


.05 
-.08 
-.2 
-.1 
-.07 
•  07 
.1 
.2 
.07 
-.05 
-.09 
-.06 
-.02 
.2 
•  3 
.2 
.07 
-.02 
-.03 
-.01 
-.004 

•  3 

•  3 
.2 
.05 
-.004 

-.007 

-.002 

0.0 

•  3 
.2 
.1 
.02 

0.0 

-.001 

0.0 

0.0 


0.0 

0.0 

-.003 

-.02 

-.07 

-.2 

-.3 

-•3 

0.0 

-.002 

-.01 

-.05 

-.1 

-.2 

-.3 

-.2 

-.001 

-.007 

-.03 

-.09 

-.2 

-.2 

-.2 

-.1 

0.0 

-.005 

-.02 

-.05 

-.08 

-.08 

-.05 

-.02 

.02 

.04 

.07 

.07 

.04 

.02 

.003 

0.0 


.09 

.2 

.2 

.2 

.09 

.03 

.007 

.001 

.2 

.3 

.3 

.2 

.06 

.02 

.003 

0.0 

.3 

.3 

.2 

.07 

.02 

.003 

0.0 

0.0 


0.0 

0.0 

0.0 

0.0 

-.005 

-.02 

-.06 

_ 

*  •  w  7 
0.0 
.0005 
.002 
.0005 
-.01 
-.05 
-.08 
-.08 
.0009 
.003 
.008 
.004 
-.02 
-.06 
-.0? 
-.05 
.005 
.01 


.02 

.01 

-.02 

-.04 

-.04 

-.02 

.02 

.04 

.04 

.02 

-.01 

-.02 

-.02 

-.005 

.05 

.0? 

.05 

.02 

-.004 

-.008 

-.004 

-.0009 

.08 

.08 

.05 

.01 

-.0006 
-.002 
-.0006 
0.0 
.09 
.06 
.03 
.00  6 
0.0 
0.0 
0.0 
0.0 


0.0 

0.0 

-.001 

-.005 

-.02 

-.05 

-.08 

-.09 

0.0 

-.0005 

-.004 

-.01 

-.04 

-.07 

-.08 

-.07 

0.0 

-.002 

-.008 

-.02 

-.04 

-.06 

-.05 

-.03 

0.0 

-.0008 

-.005 

-.01 

-.02 

-.02 

-.01 

-.006 

.005 

.01 

.02 

.02 

.01 

.004 

0.0 


0.0 

.03 

.05 

.06 

.05 

.02 

.008 

.002 

0.0 

.06 

.08 

.07 

.04 

.02 

.004 

.0006 

0.0 

.09 

.08 

.05 

.02 

.005 

.0009 

0,0 

0.0 


0.0 
0.0 
0.0 
0.0 

-.0008 
-.003 
-.009 
-.01 
0.0 
0.0 
0.0 
0.0 
-.002 
-.00? 
-.01 
-.01 
0.0 
.0005 
.001 
0.0 
-.003 
-.008 
-.OOQ 
-.007 
.001 
.002 
.003 
.002 
-.003 
-.006 
-.oo  5 
-.003 
.002 
.005 
.006 
.002 
-.002 
-.003 
-.002 
-.00? 

.00? 

.009 
.008 
.003 
-.0005 
-.001 
-.0005 
0.0 


.0! 

.01 

.00? 

.002 

0.0 

0.0 

0.0 

O.o 

.01 

.019 

.004 

.0009 

0.0 

0.0 

0.0 

0.0 


0.0 
0.0 
0.0 

-.0007 
-.002 
-.006 
-.01 
-.01 
0.0 
0.0 

-.0005 
-.002 
-.005 
-.009 
-.01 
-.009 
O.o 
0.0 
-.001 
-.003 
-.006 
-.008 
-.006 
-.004 
0.0 
0.0 

-.00 05 
-.002 
-.oo  3 
-.003 
-.002 
.0009 
.0008 
.002 
.003 
.002 
.002 
0.0 
0.0 
0.0 
.003 
.006 
.008 
.006 
.003 
.001 
0.0 
0.0 
.008 
.01 
.oo  9 
.005 
.002 
.COOS 
0.0 
0.0 
.01 
.01 
.006 
.003 

.0007 

0.0 

0.0 

0.0 
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Appendix  D 
Computer  Software 


This  appendix  contains  the  computer  code  for  the 
algorithms  presented  in  Section  II  and  III  along  with  the 
additional  program  modules  required  to  perform  the  Monte 
Carlo  study.  Programing  was  done  using  FORTRAN  77  im¬ 
plemented  on  a  CDC  6600  computer  system.  Program  LEUTER 
performed  the  executive  function  for  controlling  the 
Monte  Carlo  simulation  runs.  First  developed  by  Mercier, 
program  PLOT  was  the  executive  for  generating  the  Monte 
Carlo  error  statistics  and  associated  CALCOMP  plots. 

(Ref  13)  Each  program  and/or  subroutine  contains  a 
brief  description  of  its  function. 
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*  n  *  « o  •»  *o**«o*n*  *  ♦  •  **00000 


P90GPAM  Lf  (JTtP 


LEUTET  IS  A  MONTE  CAPLO  SIMULATION  PFCGRAH  FOR  PERFORMANCE 
ANALYSIS  OF  TARGET  TRACKING  FILTERS  WHICH  USE  FLIP  DATA.  MUCH 
n e  THC  truth  MOOEL  DEVELOPMENT  IS  TAKEN  Fl'OM  PREVIOUS  THESES  AT 
AFTT,  HIGHER  ORCER  FILTERS  WILL  BE  ANALY7EC  AtiC  COMPARED  TO  AN 
EXT-NOEO  KALMAN  FILTER  IN  LASER  FQINTTNG  AN L  TRACKING. 


REAL  0  (3 ,3)  ,IMA  X,  WORK (3 ,3),  SAVE<  14)  ,  OFOl  ( (  ,  fj)  ,00(6,6) ,  RN(64 
INTEGER  hPS,NFS,NPIX,ONE 
CHAD  ACTER  ISPTL*3 

COMMON/IN/  NRUN » TFINAL ,  SIGS1,SIGAT  ,  C (5)  ,SIGF1-  ,SI GF2 , ARC , FI MAXO, 

*  SN,SIGMFt  ,RF,SIGMA(3 

COMMON/ FLIP/  XFOV,YFOV, NPTX ,CS TH , SNT H , S IG MS ,STGFLR , ASP°0» 

*  TM c  AS ,  VMAX, RANGE-?*  RANGE  ,  IMA  X  ,SIG  VF,SIGPVF  ,  AR 
COMMON /APR AY/  X  FP  <8  )  ,XF  PO  <8  )  ,XFM(3> , PFPtP  ,  8  ) , FFPOLP ( 8 , 3 ) , 

«  FPFP(8,8)  , PFM(ti»8> ,QFD(8,8> , FXTPA(8,b) , PH  IF (8, 8)  , 

»  PHIFT<8,8) ,UT(2,l>  ,PL2P(64,2) ,E0(8,2> ,TEMP(5,3), 

t  T£MPl(8,8)  ,S CO 0(8,8) ,W( 6) ,XS (8) ,H(E4,C) , H F  (8,3) t 

t  7(e,6)fR(bH,6‘‘),WKAREA(E»,jf'),HT(6,64),PHin,3) 

C0MM0N/CHANGE/RIH(64,i) , 0X< 3 ) , GAIN ( 8 , 64) 

...''AMIABLE  DEFINITIONS... 

•  . • E Y C’CUT c  LLUTER... 


...0°EN  FILE  FOR  SAVING  PLOT  DATA... 

0°EN(8  ,  FILE=  'MYCATA  », STATUS*  »N  EW  FO  PK=  *  U  NFCR  MATT  ED  %IOSTAT  =  IOS) 
TP  (TOS  .LT.  f>  THEN 

PRINT* ,  *  FILE  MYOATA  WILL  NOT  OPEN* 

SO  TO  99 

ENOIP 


•  « .  R  r  A  0  INPUT  DATA.  .. 

RCA9  * ,NFUNf TFINAL 

p^AO"  ,  SI  GS1,  SIGMA  13,  SIGFL.R, SI  GAT,  SIGHS 
°E  AO  “ j 1MAX , ASFRO, 1SPTL 
IF  (TSPTL  .EO.  'YES')  THEN 

3CA0* ,C (1) ,C (2) ,C<3) ,C<4) ,C<5> 
FNOTP 

RFAD*,SIGFH.,SIGMF0,SIGF2 
REAR*, A  P.L,FF, FI  MAXO 

...VERIFY  ANO  ECHO  DATA... 

FIMAX-=ABS(F1MAX»*) 

SIGVP=ABS(SIGMF( ) 

CALL  INCHEK(ISPTL) 
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« o  *  «  r>  *  «  o  «  *  «  o  *  *  *  o  *  *  o  o  *  *  » o 


CALL  CH0LEX(RN,R,6*> 

PRINT  »( ///2X  ,A/>  * »  * T  HE  CHOLESKY  SOUA?E  ROOT  OF  RN  IS»' 
CALL  MWPITE(R) 

CLS- 

00  22  1=1, E4 
TO  22  J=l, 64 
R  ( I >  J) =(  .* 

22  CONTINUE 

00  11  1=1,64 
R(I,I)=1. 0 
11  CONTINUE 

FNOTF 

...FILTER. .. 

CALL  F IlTRL <  DT , PH IF ,PHIFT) 

°RTNT *( /// 2X ,  A/ )  *,*THE  FILTER  STATE  TRANSITION  MATRIX  IS* » 
CALL  HOUT(PHIF) 


PRINT  *  (A)  *, *1 » 

PRINT  5EGIN  THE  MONTE  CARLO  SIMULATION 


...TOP  OF  FUN  L  COP, • • 

no  non  l=i,nrun 
TIMEsO.C 

JPRINTsP 
PRINT •(/////) * 

PRINT*,  *****'•****  RUN  NUMBER  %L,'  ********  *» 
...RUN  INITIALIZATION... 


...ACCUISITION  PHASE  FILTFR  00  MATRIX... 

CALL  OAOUIR (OT, SIGF10,SIGF2, OFO) 

...INITIAL  COVARIANCE  P  ♦  t  H)  .  .. 

CALL  COVARt  (PFP) 

...OROVIOE  FILTER  WITH  INITIAL  VELOCITY  A  NO  POSITION 

XFP(  ii  =f.  .- 
XFO( 2) =t  ,C 

CALL  XFORM (TI ME, XFP( 3) , XFP< 4 > ,  X VEH , Y VEH , Z VEH, VMAX , RANGE) 

XFP(5)=i 

Xe®  (  *> )  =c  .r 

xpo(ti=i .r 

XFO(3)=o.T 
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<_5  »  «  4  U!  •  «  *  U  *  OOO 


... DEFINE  PARAMETERS... 


C  SAMPLE  RATE  IS  1/30  Th  OF  A  SECOND 

OT  =  1./3:  . 


...INITIALIZE  TRUTH  MODEL  VARIAP.ES... 

CA  LL  RANSET  (71682) 

ONE  =  1 
NPS  =  8 
N»IX=8 

NMS  =  NPIX'NPIX 
NFS=  8 

NFS2=NFS*NFS 

NIS  =  3 
RT=1 . /PF 
XFOV=8. 

YFOW=8 • 


...PROBLEM  INITIALIZATION... 


...TRUTH  MODEL... 

CALL  TRUTH! (DT , SIGA T ,SI GS 1,  PHI , 3D,  OD , 0) 

P°INT  '  (///2X,A/)  ' ,  'THE  TRUTH  MODEL  STATE  TRANSITION  MATRIX  IS » ' 

CALL  MOUT(PHI) 

PRINT  •  <///2X,A/)  *,*THE  TRUTH  MODEL  DISCRETE  INPUT  MATRIX  ISt* 

CALL  MOUTl(BO) 

PRINT » (///2X,A/) ', «THE  TRUTH  MODEL  QD  MATPIX  TSt* 

CALL  MOUT(OO) 

...TAKING  CHOLESKY  SQUARE  ROOT  OF  QO... 

S0O0 (1,1)  =  SPRT(Q0<1,1) ) 

SOQO (2,2)  =  SQQO  (1.1) 

CALL  CHOLEXtQ, WORK, NIS) 

00  S’  1  =  1, MS 
00  J=l,MS 
3000  (1*2, J*2)  =  WORK  (J,  I ) 

S0m<I+5,  J+5)  =  WORK  ( J,  I ) 

33  CONTINUE 

PRINT  * <///2X,A/>  ', 'THE  CHOLESKY  SQUARE  ROOT  OP  OD  IS»»  j 

CALL  MOUT(SQOC)  | 

C  ...CLIF  SPATIAL  BACKGROUND  NCISF...  ! 

* 

Ir  (TSPTL  .EO.  'YES  *)  THEN  j 

CALL  RNCIZl  ( C ,  SIGMA  B  ,  RN,R  >  f 

R°INT  *  ( ///2X  ,  A/)  ','64  X  64  SPATIAL  CORRELATION  MATRIX!  * 

CALL  MWPITE(RN)  ,! 

# 

C  ...TAKING  CHOLESKY  3CUARE  ROOT  OF  RN...  f 
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•  n  *  *0**0*  *o*  *  ♦  o  •*■  *  r  *  o  *  «  o 


...PRINT  INITIAL  RUN  CONDITIONS... 

IF  fL  .EQ.  1)  THEN 

00 1  NT  * ( ///T2 » A/)  *,*TH£  FILTER  ACQUISITION  00  MATRIX  IS » * 
CALL  MOUT (OFO) 

ot>INT  *(///T2,A/)  *,«THE  INITIAL  P(*)  MATRIX  IS** 

CALL  MOUT(PFP) 

=>°TNT*(///T2|A/>  «,*THE  INITIAL  FILTER  ESTIMATE  XF<*>  IS»* 
CALL  MOUT  2 (XFP ) 

PRINT  •{///u  (TR8,A5,T  R9>  )  *,  *  XVEH*,*  YVEH*,*  ZVEH  »,  »  RANGE  * 
PRINT  •  (L  (TP5  , G16 • 10 )  )  *  ,XYEH,  YVEH,  ZVEH,  RANGE 
FNOTF 

.  •  .RrSET  OTHER  INITIAL  RUN  CONDITIONS... 

00  UU  I=l,NPS 
XS(I)=C  •  t) 

CONTINUE 
XCEN  rR=T  . 
rc ENTO=.-  . 
elMI  N  =C  • 

THE AS  =0 

FIMAY*AES( FI MAX  0) 

A 

RftNCE'laFANGE 

FIGVP=APS(SIGMFC) 

FIGFl  =  ABS(SIGFi(J) 


...TIME  LOOP  STARTS  HERE... 


50  TIM"  =  TIME  ♦  OT 
IPRINTaJ PRINT ♦! 

IF  (TIME  .GT.  TFINAL)  GO  TO  999 
...TRUTH  HOOEL  STATE  PROPAGATION... 

CALL  XFORM (TIME ,UT (1,1)  , UT ( 2 , 1 ) , X V EH , Y VEH , Z VEH, VM AX , RA NGE) 
CALL  TRUTH <XC ENT R,YC ENT R,NPS ,ONE) 

. • ,-TLTER  PROPAGATION... 

CALL  M0VEUP(NFS,NFS2,0NE) 

...FORM  CENTRCIC  POSITION  AND  FILL  TRUTH  ARRAY... 

XPEAF  =  XS(1)  *  XS  (  3 )  ♦  XS('«)  -XFM(l) 

YPF5X  =  XS ( 2)  ♦  XS(b)  *  XS(7)  -XFM(2) 

...CHPCK  FOR  FILTER  LOSING  TRACK... 


IF(APR (XPfAK)  .GT.  3.C* ASPRO'SIGMS)  THEN 

PRINT*,  'LOST  TRACK,  X  CHANNEL,  MEAS  CALLEO  *,IMFAS,*  TIMES.*, 
»  *  RUN  NUMBER  »,L 

GO  TO  6 f 
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o  n  n  in  u"  m  *  o  *  *  ♦  o  .  *  o 


ELSET  F  ( A8S ( YPEAK) . GT.  3 . r'  4  SP RO '  SIGHS  )  Tmen 

®RTNT* , 'LOST  TRACK,  Y  CANCEL,  HE  AS  CALLED  ’,IMEAS,  *  TIMES.  ' , 
*  '  RUN  NUMBER  ',L 

GO  TO  6 f 
ELS'- 
ENOT  e 

* 

C  .  •  •  DUT  H  MOOEL  MEASUREMENT... 

CALL  MEAS (XPEAK , YPEAK, L) 

,..PCRFCRM  MEASUREMENT  UPDATE  FOR  THE  FILTER... 

TALL  ME A  SUE  (PI ,NFS,NMS, ONE, NFS  2 , F I MA X , S I G MF C , XPE A K , YPE A K) 

...onINT  RESULTS  EVERY  TENTH  SECOND... 

IF(()orxmt  ,EO.  3)  .AND.  (L  .El.  t))  THEN 
03 1  NT  * ( /////)  ' 
or,INT*  ,  '  TIME=  *  ,  TIME  , ' 

3rI NT '(///<*  (TR8, AS, TR8))  XVEH',*  Y  V  E  H  ' ,  *  ZVEH RANGE ' 
°PINT  '  <MTP5,G16.1G>  )  •  ,X V  EH, Y V  EH , ZVEH, P A NG E 
°  DI  NT  *(///2(TR6,A7,TR6))  *,'UT(t,i)  »,'UT<2,1) ' 

°  PINT ’(2<TP5,G16.1t) ) *,UT (1,1) ,UT (2,1) 

°R I  NT  '  ( ///T2 , 4/)  ',’THE  TRUTH  MODEL  STATE  ISt' 

FALL  MOUTEfXS) 

30 1  NT  ' ( ///T  2 , A / )  ',*TH£  UPDATED  FILTER  C.ST1  MATE  XF(*>  IS*' 

FALL  MOUT  2 ( X  F  o  ) 

°°TNT  '(///T2,A/>  ','THE  UPDATED  P  (  «-  >  MATRIX  IS*' 

CALL  MOUT(PFP) 

PRINT  '(///T2,A/)  ',*THE  TRANSPOSED  GAIN  MAT  FIX  IS*' 

FALL  MCUT3 (GAIN) 

J°RINT=C 

CNDI  F 

IF  (  SI GMF t>  .  GT  •  8 . 3 )  GO  TO  52 

...COMPUTE  NEW  ESTIMATE  QF  3IGVF  AND  AR... 

DO  "i  1=1, NMS 

AR=  «R*.tGl»(RIH(1, 1)*PL2P(I  ,1)  > 

STGVF  =  SIGVF*.' 01* (RIH(1 ,1)*PL2P(I,  2)  ) 

1  fonttnue 
IF(T3.LE.l.)  ar=i. 

IFfSTGvF.LE.O.)  SIGVF=S  IGMFt? 

2  OONr  T NUE 

.♦.=>ETUNE  OFO  MATRIX... 

CALL  OTUNE(OFP,TIME,OT, SIGF1) 

...WRITE  DATA  TO  FILE  TAPES... 

SAV-(l)  =  XS(1) 

S  A  VE ( 2)  =  UT ( 1 ,  1) 

SAVE (7)  =  XS ( 2) 

S4VC  {<* )  =  UT  (2,  1) 
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GA VrfK)  =  XFP ( 1 ) 

<?AVr(M  =  XFP(3> 

SA'fF  (T)  =  X  FP ( 2  > 

S AVe<4>  =  X  FP (4 ) 

^=■(9)  =  XCtNTP 
SAVEUC)  =  YCENTR 
S»V-(il)  =  PFP(1,1) 
^^(121  =  PFP(3,3> 
GAVE(13>  =  PFP ( 2  »  2) 

SAVE (14)  =  PFP  ( 4  >4) 
WRITC<8)  (SAVE(I)  ,1  =  1, 1**> 

C  ...90TT0M  OF  TIME  LOOP... 

GO  TO  5t 


C  ...FILTER  LOST  TRACK... 

4 

6!»  no  tin  j*i»14 

113  SAV-(J)=C. 

WRITE ( 8)  (SAVE(I) ,1=1,14) 

T I Mc  =  TIMfc+OT 

IF  (TIME  .GT.  T FINAL)  GO  TO  999 
GO  TO  6C 

* 

C  ...°OTTOM  OF  RUN  LOOP... 

999  CONTINUE 


99  CONTINUE 
ENOc"f L£  8 
CLOSe (6) 
c'NO 
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SUBROUTINE  MEASUR (RI »NFS  »  NFS  *  ONE,  N- S Z ,  FI  N  A  X,  SIGMFf>  ,  XPE A K,Y FEA K) 


* 


MEASUR  PEPFORMS  THE  FILTER  MEASUREMENT  UPPATE  BASED  ON 
EX  TENCEO  KALMAN  FILTER  EQUf  T  i.QN5  FOR  BOTH  l  HE  STATE  VECTOR 
COVARIANCE  MATRIX, 

, VAFI ABLES, ,  , 

REAL  PI, 71(64) ,HF1(64),3IAS<L4) 

INTEGER  NFS,NMS,ONE,NFS2 

COMMON/CHANGE/RIH(64, 1)  ,  0  X  (5  )  ,  GA I  N  (  5 ,  EL  > 

CO  MMON/ ARRAY/  XFP(8)  ,XFPO(fc>  ,XFN(«I  ,PFP(6,t)  ,PFP0L0(8,6)  , 

I  PPFP(8,8) ,3FM(6 ,8),  3r0<3 ,8) , EXTRA (6,8 ) ,PHIF<6 ,8) , 

I  PHIFT (8,81 ,UT(2,1),  3.2P(64, 2)  ,BO(f ,2)  ,TEMP(8, 8) , 

I  TEMPI  (8 ,8) ,  SQQD  (8,3)  ,  H(3)  ,X3  <?  >  ,H(6<*,  8)  ,HF(8,  8), 

I  Z(8,3>,R(6W,6<  )  ,WKAREA(5  0,51)  ,HT<8,64>  ,PHI<8,8) 

.BEGIN  UPOATE..  . 

CA  LL  SMI  FT  A  <PFPOLO,PFP,  NFS2,  NFS  2) 

X°  FA  K  =  X  FM  (7 ) 

Y3  EAK  =  X FM ( Q ) 

IF  ((XFM(3)11*  2*XFM(4)**2)  .  EO.C  .)  XrH(3) =.  )J1 

• »  ,F OFM  FILTER  CENTROIO  POSITION  A  N 3  FILL  OUT  NON  LINEAR 
S  “ALL  H.  CALCULATE  PARTIAL  SMAL.  H  PARTIAL  X... 

CALL  MEASF( XPEAK, YPEAK,ONE,FIMA< , SI GMF ?  ,BIAS) 

□0  6'  1=1, NMS 
00  6'  J=1 , NFS 
HI  ( J  ,  T)  =  H  (I,  J) 

SC  CONTINUE 

...THE  INVEFSE  COVARIANCE  FORM  IS  JSED  BECAUSE  OF  THE 
LAPGE  MEASUREMENT  VECTOR  SIZE... 

CALL  MMPY  (PFP,HT  ,H, NFS, NMS, NFS) 

PFF=  HT  *  H  (R-i  IS  SCALAR  AND  MULTI3LIEO  LATER) 

10  GT  =  I. 

CA  LL  LINV2F(PFM,NFS,NFS, EXTRA,I OGf , H< AREA , 1ER) 

EXTPA=  P(TI-)-l 
no  f>:>  1  =  1, NFS 
03  h*  J=1,NFS 

PFP(I,J)=PFP(I,J)*RI  ♦  EXTFA  (I,  J) 

SF  CONTINUE 

PFP=  P  (II  ♦)  -1=  HT  *  R-l  »  H  P(TI-)-l 
10  GT  =  0 

CA  LL  LINV2F (PF P,NFS, NFS, E XI R A, 10 GT  ,  H< ARE A , I ER) 

DO  7  1  T=1 , NFS 
DO  7  J= 1 , NFS 
PF  P  (I , J )  =  EXT F A ( I , J ) 

7f  CO  NT  INUE 

...CHECK  FOR  NEGATIVE  DIAGONALS... 

DO  ITT  1=1, NFS 
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IF  (PFP(I,I)  ,GT.  1.)  GO  TO  lil 
PRINT*, «PFP (I, I)=  * , PFP( 1,1) 
PFP(I,I)=PFPOLr><I,I> 

13!  CONTINUE 
C  PPpsP  (TI  «■) 

C 

...REARRANGE  MEASUREMENTS  TO  VECTORS... 

03  8  >  J=1 , NFS 
03  8'1  K=i,NFS 

Hcl(  K4-  <  ( J-l  I  *8  )  )  =  H?(K,J) 

Ti  (K ♦<  ( J”l)  *  8) )  =  ZK,J> 
u  CONTINUE 


...PIPFORM  STATE  VECTOR  UPDATE  JSING  BIAS  CORRECTION  TERMS. 

CALL  MAODtRIH, 71,HFi,NM5, ONE  ,NFS) 

RIH=kESIOUALS=  Z-SMA_L  H 
CA  LL  MAOO  (HF1,  PIH,  BIAS  »NMS,  ONE,  NFS) 

MF1  =  RE  SI  0  UAL  S  -  BIAS  CORRECTION  TERNS 
CALL  MMPY (GAIN,PFP,HT ,NF S  , NF S  ,NNS > 

GAIN  =  P(TI+)  *  HT 
03  8“  1=1, NFS 
03  8q  J=1,NFS2 

GAINd  .J)=GAIN(I,J)*R1 
5  CONTINUE 

GAIN  =  P(TI+)  *  HT  *  R-l 
CALL  MMPY  <0X,C-AIN,HF1,NFS,NMS,0NE1 
C  OX  =  P  ( T I  *  HT  *  R-l  *  (7  -  SMAlL  H  -  BIAS) 

CALL  MAOO(XFP,GX,XFM ,HFS , ONE , ON* > 

C  XFP=X  (TI  ♦)  =  P  (TI+ )  *  R-l  '  HT  *  (7  -  SMALL  H)  ♦  X(TI-) 

END 
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SUBROUTINE  HEAS  ( XP EAK, YPE' < ,T SUE > 


C  ...VARIABLES... 

RCAL  TKAX,Wl(f4,l),  WC  <  6*+  ,  i) 

COMMON/FL1R/  XFCV,YFOV, NPIX ,CSTH , 3NT H , S I G US ,S J GFLR , AS PRO, 

*  1M E AS, VMAX , RANGE*  , RANGE , I  MAX  ,SIGVF,SIGPVF,AR 
COMMON/ARRAY/  XFP(8),XFP0(8) ,X FM ( 3 ) , PFP ( 8 ,8 > , PFPOLD (8 , 3 ) , 

»  PPFP (8 , 8) , PFM(a,8) ,QF0(8,8) ,EXTRA(6,8>  ,PHIF(3,8)  , 

*  PHIFT(8,8) ,UT( 2,1), PL2P <64,2 ),eD(6,2),TEMP (3,3), 

*  TEMPI (8,8)  , SQOG (8, 8) ,M(8),XS (8 ) , H (64 , 8) ,HF (3 , 3  )  , 

*  7(3,8)  ,R<  64,64 )  ,MKAPE  A  (50,5  C  ),HT  (8,64)  ,PHI  0,3) 

. • • T ® UTH  MEASUREMENT... 

’MIN  =  ' . 

SIG°V=SI  GMS*RAf(GEl/RANG  E 
PLV"L  =  SHRT  (UT  (l,i)**Z+UT  (2,  l)  *  *  2) 

SNTH=UT (2,i)/PLV£L 
CSTHriJT  (l,i)/PLVEL 

SIGVa ( l.*(ASPPO«l.)*PLVEL/VMAX)"SIGPV 
I  =  ( NPIX* I SUP) 

IOIV  =  I  SUB**  2 
XINCP  =  XFCV/FL  OAT ( I ) 

Y  INC  °  =  YFO V/FL  OAT ( I ) 

X  =  -1  .  ♦ XFO V/2 .  ♦  XINCR/2. 

Y  a  YFOV/2.  -  YINCR/2. 

X°  =  X 

CALL  NOISE (64, Ml) 

CALL  MMPY(W0,P,W1,64,£>4,1) 

MN=  1 

00  K= 1 , NPIX 
DO  1*  J=l, NPIX 
MN=MM*l 

TOTAL  =  G. 

XN  a  x 
YN  =  Y 

DO  l"  N=i, ISUB 
YCS7M=  (YN-YPEAO*CSTH 
YSNT Ha  (YN-YPFA  K )  "*SNTH 
n0  "  M  =1,ISUP 

ARGS°V=YCSTH-{XN-XPEA<)*SNTH 
ARGSV= lXN-XPEAK)*CSTH4YSNTH 
ARG  =  -(  <ARGSV/SIGV)**2M  ARGSPV/SISPV)  **2>  *  .5 
TOTAL  =T0TAL4FXP(A*G)*IMAX 
VN  =  X  4FLCAT (M)*XINCP 
5  CONTINUE 

YN  =  Y-  FLOAT (N ) * Yl NCR 
XN  =  X 
13  CONTINUE 

7  (  K,  J)  sTOTAL/FLOAT(IDIV) 

800  EACKGPGUNQ  AND  FLIP  NCISE  BOTH  7ERO  MEAN 

Ir(SIGFLR.EQ.r. )  GO  TO  3C 
GA'JSS=(!. 

00  11'  LL= 1 , 1 2 
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G4U5S=GAUSS+RANF<) 

110  CONTINUE 

r=(GAUSS-5.)' SIGFLR 
7  ( K  ,  J  )  =  7<K,J)  ♦  F 

3G  7<¥,  U>  =  7<K,  J)  +W0(MN,1) 

IF(7(K,J).LT.7MIN)7MIN=2(t<,  J) 
¥  =  YN  4-FLOAT  <ISU8)*XINCR 
15  CONTINUE 

y  =  Y  -  FLCAT <ISUB)*Y1NCR 
¥  =  X  " 

21  CONTINUE 

tME4  ?=IHEAS*1 
IF<7MIN. EO.r.)RETURN 
00  2e  I=1,NPIX 
00  75  J=  1 ,  NPI X 
7(I, J1  =  7<I,  J) -7MIN+.1 
25  CONTINUE 

prTypvj 

END 


SUPROUTINE  MEASFtXPEAKfTPEAK.ISJB, -IMAX,SIGMFP ,BIAS) 

..  »V API ABLES. •  • 

REAL  IMAX,XPEAK,YOEAKfFIMAXtSIG1F!:,3l  AS<5'*> 

COHMON/FIIR/  XFOV,YFOV,NFIX,CSH,  S U H , SI 3MS , SI GFLR , ASPRO , 

I  IMEAS,VMAX,RANGE<  ,*AM3E,  IMAX,SIGVF,SIGPVF,AR 

COMMON/ ARRAY/  XFP(8)  , XFPO ( 8 )  , XF i  (  5 >  , 3 FP ( B  ,  ? )  ,  PFPDL 0 <  6 , 8 )  , 
t  PFFP(8» 8)»PFM<e ,8),  3-) (5,8) , EXT  PA <8,8 ) , PHIF  IB  ,8) , 

«  PHIFT(8,8l,UT(2,l),7.7D(6R,2)  ,  BD<  8 , 2)  ,  TEMP  (  8,  b)  , 

t  TEMPl(8,8),SOOO(fc,1) ,rf(3),XS<8),H<64,8),HF<8,8>, 

t  7(3,8),R(&4,bH)  ,  WK4REU  5  0 ,5  }  l  ,HT<8,64)  ,  PHI  <8,  6) 

...FILTER  MEASUREMENT... 

ZM IN  S  r, 

p  L  VEL  =  SQPT  <XFM(3)**2+X- *2) 

SMTH  =  XFM(L) /PL VEL 
C3TH=  XFM  ( 3 ) /PLVEL 
SIGPVF=SIGVF/AR 
I  =  ( NPIX* I  SUB ) 

IOIV  =  ISUB**2 
XI NCr  =  XFOV/FLOAT  (I ) 

YINCR  =  Y  FO  V/FLOA  T ( I ) 

X  =  -i.  •XFOV/ 2 •  >  XI  NCR/  2  • 

Y  =  YFOV/2.  -  YINCR/ 2 • 

xr  =  x 

DO  3  11=1, NPI X 

00  3  J J= 1 , NPI X 
TEMP (I  I , JJ) = J.  1 
3C  CONTINUE 
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03  2  K=1,NPIX 
N'JM  =  K 

03  1'  J=1,NFIX 
T3TAL  =  f  . 

SU^i  =  r. 

SJM2=C. 

SUH3-f  .«! 

SJMU=f  .f 
S'JM«5  =  C.D 
X't  =  X 
YN  =  Y 

03  1'  N=1,ISUB 

y:  sth=( yn-yfea  *)*csth 

Y3  NTH=(YN-YP£AK)*SNTH 
03  5  ¥  =1,1  SUP 

ARGS°V=YCSTH-(XN-XPEAK>>  SNTH 
ARGSV=(  XN  -  X  P  E  A  K)  *  C  ST  H  »Y  3  N  T H 

ARG=-(( ARGSV/SlGVF)**2+< A kGS PV/5I 33 ) **2> •  .5 
PART=  EXP  ( A  RG)  *  FI  MAX 
TOTAL  =  T OT  AL ♦PART 

DA  RT  t  =  (  A5GSV+CSTH/SIGVF**  2-AFGS3V*SNT  H/S I  3  P  V  F* *  2) 

PA  RT2=(ARGSPV*CSTH/SIGPVF»>-  2  ♦  AR3  3  Y  »  S  ^ TM/ 3  I GVF*  *2) 

S J  HI  =  SUMl+PART'PARTl 
SUM2  s  SUM2+PAKT‘*°ART2 

SJ  M3  =  SUM3-PAPT*  (CSTH*»  2/SIGVF**  >f  S'JTH*  »  2/SIGPVF*  *2)  ♦ 

I  (PARI 1*P ART  1* PART) 

SUMI+  =  SUMU-PART*  (SNTH**2^SlGVF*»2t33TH'»*  2/SIGPVF*- »  2)  ♦ 

I  (PnRT2'°ART2*PA?T ) 

SJM5  =  SUM;  +  PA  FT" ( -OSTH*  SNTH/SISV^*  *  2  +-CS  T  H  SNTH/SISPVF**2)  ♦ 
l  (PART 2*  PART 1*PART ) 

XN  =  X  ♦  FLOAT(M)*rXINCR 
5  CONTINUE 

YN  =  Y-FLOAT(N)  *  YI NCR 
XM  =  X 
IT  CONTINUE 

HF  (K ,  J)  =  TOTAL /FLOAT  (I3IV) 

IF  (SIGMF*  ,GT.r  .)  GO  TO  15 

PL2P<KMJ-1)*6,1>  =  PART*  (-(AKG5»V/>i;VF>  *  * 2» AR) 

PL2P(K-M  J-L) -*6  ,2)=  P  ART  *  (  (ARGSV*  *  2  A  RGSP  V  *  2*  AP  *  2 > /SI  GVF*  *  3) 
IE  CONTINUE 

IF  (Hp(K,  J)  .LT.  7MIN)  ZMIY  =  HF(  K,  J) 

H (  NUH ,  1 )  =  SUM1/FL0AT  (IDI  V) 

H(NUM,2)  =  SUM2/FL0AT (IOIV) 

H( NUH ,  3)  =  C  . 

H(  NU*,!0  =0. 

H«  NUH, 5)  =ti  « 

H ( NUH ,6 ) =  r  « 

H(NUH,D=H(NUM,1) 

H( NUH, 3) =H(NUH,2) 

T E MP  ( 1 , 1 )  =  SUN3/FL0AT(I0IV) 

TE  HP ( 1, 2)  =  SUM5/FL0AT(IDIV) 

TE  HP  (  2, 1)  =  TEMP ( 1 , 2  ) 

TEMP  (2,2)  =  SUM4/FL0AT(IDIV) 

TEMP  (7,7)  =  TEMP  ( 1 , 1 ) 

TE  HP (7, 8 )  =  TEMP ( 1 , 2 ) 

TEMP (8, 7)  =  TEMP( 2, 1 ) 

TEMP  (8,  8)  =  TEMP  (  2 , 2 ) 
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CALL  BIASCT ( 91 A S < NUH ) , TE M P, T i HP L , »- H , NPI X ) 

X  =  XN  ♦float<isu<3)*xin:r 

NJM  =  NUM  ♦  NP IX 
15  CONTINUE 

Y  =  Y  -  -loakisub)*yin:r 

X  =  XI) 

2C  CONTINUE 

IF  (7MIN.EO.*.) RETURN 
03  217  I  =  i,NPIX 
DO  2r;  J  =  1,NPIX 
Hc  (I  ,  J)  =HF(I,  JWMIN+fl.l 
25  CONTINUE 
RETURN 
EH  C 


?'mTni<r  91 AS  3  T  <  9 1 T  ER.  H  TE'I3!  *  PFM»MPIX) 

OtASC  COMPUTES  THE  INOT'/IOJAl  BIAS  OOnPfcC’iON  T  EFH 
JSI'jr,  THF  SECOND  »ARTIA_  nc^XVATW-:  MATRIX  CONTAINED  I N  7EM^>. 


,  . .  y/'  ’IA  BL^S. . . 

?  =■'. L  °TTtRN,TEHP  (3,8)  ,TEMPi ( < , P>  ,=?'U'cffi) 
...CALCULATE  R!  A S  CORRECTION  TEH  5  .  . . 
’Tr-'Mr  . 

"ALL  UUP  Y (TFMP1 , T£HP,Fr1  ,NPT  X,N^r  X,kHIX) 
BO  t  T  =  1 , N  F I  X 

BTT^fM  =  01T ERH+T EN°t  (I»T' 

TOSTTMUE 

BITE’M=L  .5' PIT^RM 


0T ▼•RH* 1 /2*  TRACE( OH2  X  DX2  "  D(-)  > 


00-.0*  *  kO»  U\  ft  *  O  X  v  O  .  X  O  .  •  X  O  * 


SUBROUTINE  STA7LN(XPEAK, YPf  AK,NFS,NMS,ONE> 

STATLN  COHPUTES  THE  REQUIRED  OOHDITIONAL  EXPECTATIONS  AND 
MATRICES  REOUIPED  FOR  FILTER  MEASUREMENT  UPOATE. 

...VARIABLES... 

RE  AL  CP0EX(3) ,CP0FY<3) ,:PDF( 3,3> ,H0RM, XPEAK,  YPEAK, TEMP  2(64, 8), 
I  HXHAT<64,e> 

INTEGER  NFS  ,  NMS ,  ONE 

COMMON/ ARRAY/  XFP(d)  ,XFP0  (6 ) , XFM ( 3 1 , PFP ( 5 , S )  ,PFP0L0(8,8>  , 

I  PPFO<a,8>  »PFM(8 ,8) , QFD (3 ,8) , EXTRA  (8,8) ,PHIF  (8  ,8) , 

«  PHIFT(8,8),UT(2,1),3L2P(64,2) ,BD(8,2) , TEMP (8, 6) , 

»  TEMPI (8,8>,  SQOD  (6,3)  ,  4(3  )  ,XS  (H  ,H(S*,8)  ,HF(8,6)  , 

«  Z  (8,3)  ,R(64,6*  ) ,HKAREA(53,S3)  ,HT(8,E4) , PHI (8, 8) 

CDMM0N/CHANGE/RIH(64 ,1)  ,  OX  (8  )  ,  G4 1  4 (  3, 64  )  ,  HSL  (IT,  1C  ) 

...INITIALIZE  VARIABLES... 

00  5  1=1, NMS 
00  5  J=  i ,  NFS 

H  >HAT ( I , J) =  .1 
CONTINUE 
00  1  1=1, NFS 

00  1  J=1 , NFS 

HF(I,J)=r.r 
C  CONTINUE 
HORM  =  T.r. 


...COMPUTE  VALUES  FOR  CONDITIONAL  PROBABILITY  DENSITY  FUNCTION. 


...DETERMINE  X F£A K  SIGMA**2  AND  Y^EAK  SISMB*«2... 

X=>SIG=PFM(i,l>  +PFM(7,l)fPFM(l,D  ♦3rM(7,Z) 

Y3  SI G=PFM (2,2)+PFM(8,2)*PFM(2,E) *P-M(b,3) 

...FIND  PIXEL  CONTAINING  XPEAK  ANO  YPEAK... 

IF  (  (ANINT  (XPEAK) -XPEAK)  .LT.  ’ .  li  >  MEN 
X  =ANINT (XPEAK) ,5 

e.  se 

X'  =  ANINT (XPEAK) -0  .5 
ENOIF 

IF  ( (ANINT( YPEAK) -YPEAK)  .LT ,  „ ,  d  >  THEN 
Y  l=A  NI  NT  (YPEAK)  ♦*) . 5 
ELSE 

yrANINT  (YPFAK)  -1.5 
ENOIF 

...COMPUTE  VALUES  FOR  X  OIPECTION... 

00  1'  T=-l, 1 

A  ?GX  =  -v. 5*  (  (( (XI* FLOAT  (I)  )  -X 3 E  A  <)  f  *  2) / X  PS IG ) 

C  FDFX(I*2)= (1. 8/SORT (XPSIG))* EXP (A RGX) 
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ARGY  =  -f  . f- ( < ((YC+FLOIT (J ) ) -Y »E 4 < ) * *2 ) / f PSIG ) 

CpriFY(I+2)  =  (i.  i/SQRT  (  Y  PL  1  G) )  *  E<  »  ( ARGY) 

15  CONTINUE 

* 

C  ...EVALUATE  AND  NORMALIZE  PIXEL  Vi.UES  Or  CP  OF. . . 

OD  2r  1=1,3 
DO  2r  J  =  1 , 3 

C  FCC (I , J)=CPOFX  <I)*C’DFY( J) 

NORM=NORM*CPOF(I, J) 

25  CONTINUE 

00  3  1=1,3 

00  3"  J* 1 , 3 

CPOP  (I,J)=CFOF(I,J)/MORM 
3P  CONTINUE 

PR INT* 

PRINT-, 'THE  STATLN  CPOF  IS»' 

CALL  MOUT  <CPOF ) 

* 

C  ...COMPUTE  CONDITIONAL  EXPECTATION  OF  ME A SUR EMENT , HHA T . . . 

C  HF  =  HHAT 

» 

no  1=1, NFS 

00  <*r  J=1 , NFS 
00  AC  K= 1 , 3 
no  A  0  L  =  1, 3 

HF(I,  J)=HF(I,J)  ♦HSL  (I*L«1,  J-<«-3)"C30F(K,L) 

AC  CONTINUE 

A5  CONTINUE 

m. 

C  ...COMPUTE  CONDITIONAL  EXPECTATION  0“  H  ‘  X,  HXHAT... 


C  ...FIND  PERCENTAGE  OF  OME  PIXEL  SHIFT  IN  X HA T  IS  DUE  TO 

C  DYNAMICS  AMO  THAT  OJE  TO  ATMOSPHERIC  EFFECTS... 

DEN0M1=SQRT(PFM<1,1)  )  ♦SORT (F FM( r  ,  f  \  ) 

OE  NO  M  2=  SORT  <PFM(2,2)  )  *S2  RT  (PFM(S,3>  ) 

OE  LXOY=  SORT (PFM(l,l) )/DENOMl 
OE  LXA  T=  SORT  (PFM(7 ,71  )  /0EN0M1 
OELYOY=SQRT  (  PF  M  ( 2 , 2J)  /OE  NOME 
OELYAT=SQRT (PFM(fl,8) )/0EN0M2 

* 

C  ..  .FIND  HXHAT.  .. 

* 

OD  5  1=1,3 

OD  5  <  J=1  »  3 

* 

C  ...FINO  APPROPRIATE  ’ART  OF  HS.  AMO  REARRANGE  TO  A  VECTOR... 

A 

no  35  K= i, NFS 
00  55  L  =  1 , N FS 

T  EMP ( K, L ) =H  SL ( K+J-l,L-I+3) 

55  CONTINUE 

00  SC  K=1,NFS 
00  5>f?  L=  1,  NFS 

RIH(LMK-1)*8,  U  =  rEMF(L,<) 
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>  O  '  ^  « o  *  *  o  *  *  o 
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CONTINUE 


...FINO  APPFOPRITE  XHAT... 

OX  (1)  =  XFH(1)  MI-2)*OELXDY 
OX (2>=XFM(2)  *( J-2)*0ELYDY 
OX<?>  =XFM<  3) 

OX  U)  =XFM  ( '•♦ ) 

DX(5)  =  XFM(5) 

OX  (5) =XFH(6) 

DX(7>  =  XFMt7  >*<I-2)*OELXAT 
OX(8)=XFH(8H-(J-2)*DELYAT 

...WULTILY  MEASUREMENT  WITH  XHM-... 

CALL  HMPY(TEMP2,RIH,DX ,NMS,0NE,NPS) 

...MULTIFLY  SY  ANO  SUM  OVER  NINE  VALUES  IN  CPOF... 

00  5  5  K=1,N>-'S 
00  55  L=1,NFS 

HXHAT(K,L)-HXHAT<<,L)+TEMP2(<,L)»C^DF(I, J) 

5  CONTINUE 

r  CONTINUE 

...COMPUTE  COEFFICIENT  SCRIPT  H,  N  •  « • 


C  .. .REARPANGE  HHAT  TO  A  VECTOT,., 

* 

DO  7"  *=1,NFS 
00  7  '  L=1,NFS 

RI H(L*  <  K-l) ’ 8,1)  =  HF(_,  K) 

7'  CONTINUE 

■f 

C  ..  ,H  =( HXHAT  -  (HHAT  *  XHAT))  »  P"N  ... 


CALL  MMPY(H,RIN,XFM,NMS»0NE,NFSI 
C»  LL  MAOO(TEHP2fHXHAT,H, NMS, NFS,NrS> 
CA  LL  MMPY (H,TEMP2,PFM,NMS,NFS,NPS) 

END 
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o  o 


I 


.  '«rt 


I NC H£  K  ( IS  PTL ) 

TNC.HEK  ECHOS  THE  ITPir  T \  PS:?Fj‘'MS  CuME  INITIAL 

CHECKS. 

C  ...'/'P  TAPLtS... 

* 

JC’AL  TMAX 

'MAP/.  r-c;i  ISFTL*  3 

COMMON /IN/  NR  UN  ,  TEINALt  3IG3l,SI3AT,  0  (I  )  ,  5K,F1 .  ,SIGF2,A<  ,firr  , 
t  SN,SIGMR  ,RF,  STOMAS 

lOIM'-'N /FLir-/  XFOV,YFOV,NPIX,|,'ST-l,  ST TH,  SIGHS,  SI  C-FLk ,  A  SP°  0  , 

!  IH£AS,VMAX,RANGri,<ATGE,IMAxtJIGVF,SISPVF,4R 

C  ...CALCULATE  SIGNAL  TO  NOISr  RA  7 1  D . . . 

?N=SrGMAO/] pAX 

r  e  (  3  N  .  L  E  .  •  .)  SN*.  J.  1 
'N  =  l.  /CN 

C  ...rm  ]  N=UT  OAT  A  . .  . 


39i-r 

’PIN’ 


3?INr 
3  P I M" 

3  ?  r  n  ' 
33rN’ 


t 


3  P  I N” 
3  a  r  y~ 

3  PINT 
3  ?  I  N  T 


(A)  ',  '1  * 

C  T'  i,A//T*‘3,A////r+!  ,*)  *, 

°  f-  G  G  -■  A  M  LEUTER-  A  MONTE  C4E.0  SIMULATION', 
''AFT.  LFUTHAUSER,  GA-MO’, 

*4  '.-*»*<«  input  0  A  T  A 

( /////T2  »  A  »  T2»  >  A/ T  *>,  A )  *,  'VJMSEi  UF  MONTE', 
rlf  Al  T I ',  'CARLO  PlJNF' 

</Ti  ,1-  ,  T22,F6.2>  *  ,  NR'IN ,  TFI  \'A L 

(//////T2,  A)  *,  MRJTM  model  I  TPJTS . * 

<//T2  ,A,  T2'-,  A,  U6,  A,!}*,  A,  T5  ,  S  >  *, 

DYNAMICS*,*  TACKG  =  DJNC  NOISE', 

FLIR  NOISE*,*  ATMOSPHERE', 

pt:  PE  NOICJLAR  VELOCITY* 
TTl.,A,T32,A,TSA,A,TT?,A,r=3,A>*, 

SIGMA  *,  'SIGMA  *,  'SIGMA*,  *  SI  GM 
</T2,tiT.i  , T2“, El’.l',TGS,£l/.l 
TG>1,SIGM43,SIG FIR, ST GAT, SIGNS 


A ', ‘SI 


,T6 


,  El  .If  ,  T9  ,ilT«L  I 


(//T2,A,T2R,A,  TE1,  A)  *,  *  -AXIMIJM  INTENSITY*, 
ASFECT  RATIO  *, ’SIGNAL  TO  nOISE  FAT’O* 
(/12,H7.1‘  ,T2b,E17.i',»T~l,:Lr.  1  )  '  ,  I M  AX , A  SP  RO , SN 


re  CP°U  ,EO.  *  Y£i  *  )  T  -|  cn 

P'TNT* (//TP, A)  *, 'SPATIAL  NOTGE  0 0 E FF 1 1 1  ENTS  ' 
o3TNT  »(/l 5, A3, E13.H, T  2P,AT, -13. ? ,T  S  , A i  ,  E 12 . C ,T I  2 , A  3 , El  3 . 0 , 
1  T9'- ,«3,H13.6)  *,  *1=  *  >  C  (  1 )  »  *  2  =  ',C<2>,'3=  *,C(3), 


J 


»G(m)  f 


F 


(M) 


ENlf 


3-?T'r  *  f //////TE,  A)  'FI.TF?  TNPJ1  5 . ' 

3  PIN"  *  T//T2  ,A  ,1  2<  ,A,Tl  b,  A,TS^  ,A)  *,  '  MYNA  "ICS  *, 

!  «  ATuOS'PHERE*»  *  *  S  A  G  J  R  E  ME  Y  T  ' ,  *  VELOCITY' 
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o  o  o  o  n  o  o  o  n  -  o  '  •  o  r>  o  o 


33TS'  *  <  T  a ..  ,  A  ,1  72  ,  A, T 54,4  ,T  ,  A)  *,  *  SIGMA  ',  *S1  GM*  ',  'SIGMA  %  *31  OMA  ‘ 
3°I'l*M/T2fFi7,l-  ,T24,^L7.i  -,T43,£ir.i  ,  T  6  ‘  ,  l1  .!')«, 

CTGFl  ,SIG-2,t\F,SISMF 

3PTNT' C//T; ,A,T2U,A)  M'XTMJA  INTENSITY  *  >  '  ASPECT  'ATIO* 

3  M 'T  •  (  /T  2, 1 1?.  i"  ,T2b,Et  ^.l’  >  *  ,Pl  -AXi  ,  AR; 

30TM' • (A) •, *l» 

rin 


5'P^?'JTXn£  T~UTH  (OT  , SI S AT , ~ TGS 1 , 3 H I ,  <1P,  13,0) 

"’UTH  FQ-M'JLATES  rnc  r^jm  hjQEL  STATE  ’H,NSITI0N  *AT->iy, 
3H-,  rue  QTCCEErr  INPUT  MATRIX,  3D,  AND  TH£  01  L-CRE  T  E  NOISE 
Sr^E/iST-w  MATRIX,  OD.  DE*IV4TTqn  0-  r-lis  F?'“!JL/,1  ION  IS  3°E- 
IN  THESES  3Y  HERO  If?  A  HO  RY  44RNLY  -NO  JENSEN. 

3 'Ml  nr  ,  SIGA  T.SIGSl,  PHI  <  9  ,  H )  ,  3D<  3  ,  2 >  ,  00  <  :• ,  •  >  , 0  <  3 , 3  ) 

PT  A  PL  E  DEFINITIONS... 

*  r  A  J  i  -  CONSTANT,  ATMOSPHERIC  JIM£i  SHAPING  Fit  TER 
STA’P-  CONSTANT,  ATMOSPHERIC  JITTE<  SHAPING  FIlUP 
'  T  A  T  N  -  GAIN,  A  ]  M  03PH  EHI 0  JI'rT:P  S9'-»lNG  'IK" 

?  t  S  A  r  -  T :  U'r  M  H(  OrL  RMS  4  TNO>  r’Hr  3  I  ;  ERPf.R,  N  FIT 

>  T  -  TRUTH  MODEL  RMS  DYf-AMTcs  ERROR,  INPUT 

jr-  lA-fl  SAMPLE  RATE ,  I  9  PUT 

3Hf-  -ta] £  transition  matrix.  ojt=jt 

to-  OTRCfETt  1  N3  UT  MATRIX,  O'STPJT 

On-  1TSC5ETE  NOISE  STRENGTH  MA  T  R l * ,  OUTPUT 


C  .  .  .  TE  CTN£  PARAMETERS... 


4  TS'M  =  11  ,  i- 

'TA’PsTn.: 

STM'I  =  .  3'..  i  F  3  3^  *  SI  0  AT 

0?.Y=-1.  • OT 

•  \2r-  (  A G A  IN'  *  2)  *  (ATAU1"  *■  2)*  <  ATAU2*  '  '♦ ) 
”  A  R  r  E  =  AT  A IJ 1  -  A  T  A  U  2 
?4RT?  =  AT  A  Ul  ♦£  T  A'J2 
-ACT’  =  z,  f 1 AU2 
H  =  r  ACT / <  F ACT!*  ‘m) 

O?  =  r  ACT  /(FACT  1-'  *3) 

;■»  =  TACT/(FACT1'  *2) 

»«  =  l.-  LXP'.Z.  ATAUi'OSLTT 

32  =  1.-  EXP(PACT2*DELT ) 

>  T  =  '  .-  EXP  (  2  .  ’ATAU2f0TLT) 

?'a  =  nr*E XP  (  DELT'  FfiCT2) 

?"  =  nT*-_XP(2,  ATAIJP-*DE-T) 
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,  .  .  7rP0  GUT  MATRICES  •  •• 


OO  l  I=l,5 
TKM)--' 

=<“T  (  T  ,  2  )  =  t  .  C 
O'*  t;  J=i,e 

op<i,  j>=  ,r 
c  hi  ( i ,  j )  =  j . ; 

:nMrrM»rE. 

CTLL  OUT  TRUTH  MODEL  PHI  MATRIX, 


’Hr(i,i)= 

i  • 

3  H  T  ( ,  ■> ) 

=  FH]  (1,1) 

5^r (*, 7) 

=  ' E  XP ( A  TAU1*  DE_  T) 

’nr (  ,'•  > 

=  t  XP ( A  T AU2*  OE _  T) 

5  H I  (  ,") 

=  Dl  -  PHI  (L  ,H  ) 

5  H  T  (  =  ,  =  ) 

=  FHl  (  4  »  *) 

3  h  :  ( % ,  >- ) 

=  FHI  (3,  J) 

5H!(  ,-) 

=  F  HI  ('♦,>♦) 

3 nr  r , p ) 

=  FHIU,'T) 

3HM  •  ,<*> 

=  F HI (3,5) 

CT L L  OUT  DISCRETE  1N3UT  MATRIX 

3  "Ml. ♦  )  =  DT 
n (’i  ?> =  gt 

CI  LI.  THi  or  i-MRlX  W[TH  VALUES  J  S  [  NG  "  X-*  C  T  TMtGkATTON 
"10(1.1)=  S1C-S1 

on  (3,  ?)  =  fira.i) 

00(3, 7>  =  (Gl*  F  i)  /{2,*nr  Aui) 

''r*(!,'>1  =  :  V  ( G2  /(r  ACT  2**  2-Gi  'FACT  l)  G  2  X  F  A  CT  2 

00(7,  r)  -  GO-  ?  2/  F  ACT  2 

3  n  ( •, ,  ■*)  =  r>  c>  <  3  ,  *♦  T 

OG  (.,'■)  =  f  2'  (Gl/ PACT  3-2  ,J-0?/FACT!  •  ‘  2+2.  •  G  3/ FACT  3'  »  3)  - 
»  R*‘f  G?/AT>MJ2*G3'  GT/FAOT  3-2. * G 3X ? A C T 3 ‘  ?) 

0O(L,  =  )  =  w  3  (G3/FACT3*-*  ?-G?'FA;r3> -RL-!  G3/FALT? 

O0(",7>  =  '>0(7,5) 

0  0  (  " ,  L )  =  OD  ( 1  ,  5  ) 

30(0,  G)  =  '  3  G3/FACT3 
in  2  T=  • ,  v 
)T  T  1  =  3, 1 

00(  T  ♦?  i  J  ♦  5)  =  30  (I  ,  J) 

Of T-2, J-2)=DG (I, J) 

:  nvrr 

-MO 


0000*0  *00 


^n^'JTJNE  C  GVaR'  (PFP) 


c  rn\jn\  initializes  the  fiue;  covariance  ‘-uthix 

c  *Hr  bTMT  Cr  EACH  RJN. 

?r9L  a  cp <  o, p  ) 

C  . ..VT^TAPLw  DEFINITIONS*  •  • 

C  sea-  r-Li^  CD  VARIANCE  AFT^  U°DUE,  OUT3iJ 


C  .  .  ,  7"n  DU7  MATRIX*.  . 

X 

ID  l  7=1, b 

DD  1  »=i,r 

3r°  a,  j>  =  . 

1  'D'JfNllE 

C  eTll  IN  F*.  ar  TIME  V 


i)  =  2f  . 

3  eon. 

? J =OFP(l 

,!> 

3P3(*, 

T)=2  , 

3F3  (  , 

• >  =  PFP  (3 

,3) 

= 

ii 

t 

:  r  3  (  . 

)  =  PF  P  ( 

•  3) 

3  C  3  (  ~ 

*)  =  .? 

5F’(’, 

•*n 

t )  =ppp  <; 

,7) 

?i|?>3VJTIN£  r  NOi  Z‘  (C,SIG“UG,RN,F) 

^NOI7  FORMULAT  ES  THE  r^jy-i  -DUEL  R*  C  ^  G  I-  OLNP  FlIR 
y  a  a  tt  *  l  NOISE  MATRIX,  RN, 

>r1L  ~ <  ?)  ,3IGM2T,RN (b«,3  O  ,R  (6u, ) 


i  «  •  v/'r>rAPLi  OlFINITION.  .  . 

of  spatial  noise  cdcffi:  t  ents,  input 

TRUTH  MODEL  RMS  BACKGROUND  NDISE  Sr RPF ,  INPUT 
?M-  “OATIAL  N0I5L  CO  RRt'_  ATTOM  CO  1  ^  r  1 :  1 1  s|  1  MATRIX,  OUTPUT 
?-  r'fP.NAL  W0C<  AREA 
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o  o 


^~T  up  spatml  noise  oo1  ">rLArioj  ;gpfpicien'  katpix 

«l  =  3  \ 

A  -  ■» 

3  O  ■»  '■  T  =  ]  ,  N 
? 

r  r  ('.PE.6  )  GO  70  36 

?ci,T*r)=c(ii 

rc  (".GE.f.M  GC  TO  2t 
?  (  T  , '  *?) =C ( 7) 
f  c  (T  .  GE.  j  .  )  GO  T  O  3  6 

?  f  r,  r  *■' )  =  cu ) 

* ( T , ' *~ ) =C< 2) 

5  (  t  ,  r  *  A  )  :  C  <  1  ) 

UT,'  +•>)  =c  (  2) 

’ 7 I,r+« .  )=C(-) 
rr  ('.Gfc./,-}  GO  7  J  36 
7  f  *  ,  T  *  l  4  >  =  C  ( 6 ) 

?  (  T,T*tr  )  =C  (  .) 

?  (T,t  *ie-)=C  (37 

?  <  r,  f*iv )  =:  ) 

'ft,' *18) =C  (1 ) 

!6  :nNrrMue 

1 0  7  ’  J S 1  ,  M 

’  M*--’ ,2*  l)= 

’f9*:-’, 3*1-1)=;.  3 

:>=■  ,  j 

rcr  f’,G£.,6)  GO  TO  37 
’  (  *‘r  ,  3*  I  *1)  =  .  J 
?  f  n*r  i  *o)=.  <  . 

:  +„)=,. . 

7*--*,^  I  ♦(?)  =  ,.  : 

rc  (r,f: ;f.;>  go  TO  3”* 

M0*r,«»I+9)s’  ,  > 

7  ( 9 * r ,  *  *  1  * : •  )=  ,•• 

’  f  «  •*  -1  ,  *  -  L  f^)  =  ,  .  1 

r  r  r  «  r’t.  6)  GO  TO  37 

»fv:,v>i7)s  . 

5 (9“r , a»: fie)  =<  ,:■ 

5  f 1  *:  - 1 ,e  *1  *17 )  =  • .  . 

27  '  injr  ~  *j‘i£ 

TO  7'  *=i,N 

.  -  T  * ' 

70  27  J=L,N 
9  (  J  ,  ’  )  -  K  ( I  ,  J  ) 

32  lONTTN’IE 

7  0  2"1  1  =  1,  N 
7  0  3-  1=1,  N 

( I,  J)  sSI&p-AB*  R  ( I ,  J  ) 

29  T  ^  M  T  ^  M M  E 

r  -m 
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o  o  o  o 


C 

C 


C 


C 


c 


r 

c 


5,JRRTJTIS£  FllTR  )  (OT  ,PH  I  F,P -l  TFT) 


PTLTfc  FORMULATES  T  HF  ctLTER  STATE  IRANil  iCN  MATRIX, 
’MT-,  ANO  ITS  TRANSPOSE,  PHT  CT . 


?“*,L  OT  ,PHT F <E,3 > ,PHIFT < 5,E) 

.  ..'/ARTABLE  DEFINITIONS.  .  . 

'TVJ1-  CONSTANT ,  ATMOSPH EF I~  JITTEX  SHAPING  FILTER 
T-  SAMPLE  HATE ,  INPUT 

3HTe-  FILTER  STATE  T  RAMS  I T 1 0  *■*  MATRIX,  OUTPUT 
JH:rf-  PH i F  TfANSPQSE,  OUTPUT 


.  ..r»“cINE  PARAMETERS... 

A  T  VM  =11*.  1! 
rMU’  =  l.  / A  T  A  U 1 
i"LT=-i.  *P7 

. . . 7'30  GUT  MATRIX. . . 

^  !  I  =1  ,  r 

1r’  1  1  =  1, •* 

p-|TP(I,J)=  .  f 

TOMr’-,„JE 

PT LL  CUT  FILTER  PHI  MATRIX 
3  M  I  -  C  t 

3urTM. 
p  h  r r  ( ? 

3MT  =  f  ? 

3  H I  p  f  ? 

^rc(7 

3  M  r  p  ! 

3  H  l  p  (  A 

3MT  =  f  r 

3  H  I  e  ( 

3  HT  c  (  * 

3  M  I  c  (  ° 


3)  =OT 

5i)  sHT* *  E/2. 

2)  =FHiF(  1,1) 
l- )  =DT 

6)  =  PHI F ( 1 , 0  ) 

3)  =PHIF(1, 1) 

5.)  =OT 

A ) =  PH  I F  (1,1) 

c.)  =nT 
F)  =1. 

6)  =1  . 

7)  =EXP (TELT/FTAJ3  ) 
6)=pHIF(r, 7) 


C  r  I  LI  n,IT  PHIF  TRANSPOSE 

T  0  ?  1  =  1,  i 

TO  ?  ,J  =  i  ,  *■ 

pi T  CT ( I , J) =FHIF{J,I) 
2  ;OMrTNUE 

-NT 
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<  3  O  n  '  O'!  O  O  O  O  O 


?MPRVJtinl  PAQUIF (UT  ,Sl 3  FI  ,^IGf2,ir3) 

OSQUIR  FORMULATES  THE.  A  OQUI  S I r l ON  PM4:'F  F'lTE 
HS^^E  NGISF  STRENGTH  MAT^TX,  1-D< 

nTjilSFlt  ,SIGF2,OF3  <P,V 

.  ,  .  '/*•  STABLE  DEFINITIONS... 

'  T »  J|  -  CUNITANi,  ATMOSPHERIC  JITTER  SHAPING  FILTER 
IT-  OATA  SAMPLE  (ATE,  I X  °UT 

STCFf-  INITIAL  FILTER  RMS  O^AMICS  ERf-CR,  INPUT 
'  T  T-7  -  FILTER  ^MS  AT  MUS  3  Hr3T  0  rRT.3<.,  TN»J1 
’H.  nT  SORE  T  c  KOISL  STRENGTH  -1 A  r  R  I  <  ,  GUT=J~ 


C  ...O'PTU-;  PAR*  PETERS.  .. 

* 

A  T!.J*  =  1 1  , 1-. 

■nu?s1  /AT  AU1 
DFLT=-1 •  •  ’ D  T 

.  =  4PS{SIGF1T) 

C  ... 7~PO  OUT  MATRIX..  . 

TO  t  T  =  i,j 
30  i  J«x,j 

Or0(l,j)=  . 

1.  TONTtmiie 

C  . ..P'LL  OUT  FILTER  OISCR-TL  orn  MATRIX 

C  ns  START  OF  ACQUISITION  r,HA3b» . . 

T  r  0  ( f , f ) sOT*-5* 3  IGF  1/2" . 

,  3>=T3TV'  «-  SIGF1/8. 

•»rn( :  ,*|sOT‘  7'  3 IGFl/fc-. 

Tc0<  ?,  '>)  =QFC  U  ,1) 
mt',1  )=QFT(1,3) 

TrC(0,'f)=QFr(l»3) 

',cro(  •  ,  j  )  sQFD  1 1 , 3 ) 

T  r3  (  7  ,  7)  =0T‘-  •  7'  SIGF1/3, 
tf->O,")=0T-  *  2'  3IGF1/2. 

, 2 ) =QFP (3 , 1 ) 
m(  v  I  =OFC  (  3 , 3  ) 

■*r-)C4  ,fi)=iJFDC?»5) 

IF  or  ,  <  »=QFC(1 ,3) 

T^or  ,  7 )  =  OF  P  ( 3 , 5  ) 

■)F0("f-)=S5.GFl*OT 

Te-3(N,2)=QFr(l  ,5) 

T“Or  ,  :■  )=0FD(3,5) 

T(70r.,f.)=DFO(i,5) 

TF-i  f  "  ,  7)  =  (SISF  >**2)  *  (  l.-"XP(2.  »  OE.  T/FT  AU2)  I 
Tc0  '  9  »  6 ) =0  F  D  </,?) 
i  MO 
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'l'nRJ'JrINt  TkUT H (XCENTR,  YC  'lT^  ,  M  ?  5  ,  0  X  E  ) 


T 1  in h  FErFj-.is  the  T(, j t w  mod-:,  si,,,jla  ion  for  each  step. 

.  .  .  7*.PTA°LE.S  . .  . 

?  ca  L  x" tNTK  , YCFMTR 
rvjri'f^  N  PS,  ONE 

10  RAY/  yPP(!j>fXFpom,XF3H),PFF<?,  )  ,  P  F  FOl  0  <  8 ,  j  )  , 

5  ppFP<fcfa),3FM(8,J),3F3(3,t),EyTr'A(t,o),pwlF(3,3), 

•  pHIFT (3,8)»UT(?,i),P_iP(64,c)  ,  B  C  (  ,2) , T tMP (6, 3)  , 

?  TE-t°l(3,t),  SOOT  (  3  ,  3 )  ,  M  ( »  )  ,X5  <  )  ,  H  <£.  *,  i  )  ,  HF  (  3  ,  3 )  , 

S  7  (3 ,3)  ,  K(3  ♦  ,(  '■  )  ,  WKA?;U  *  ,c  •  )  ,HT  (  •  ,  fc  t* )  ,  PHI  (  8,  3) 

.  . .  p-''c'QFM  TRUTH  MUOEL  5  IMIIL  ' TI3M . . . 

<'r^"3  =  XG'MR*Ur  (  t , 1 ) *  H3  (1,1' 
r?r  =  -y:chi  k+ut  <2,i)-B3<2,?> 

MI.L  ”  R1E  E  (NFS,1-!) 

''LL  "  M  F  Y  ( T  f  HP,  3  010,  W,M3S,Mr'^,O^E) 

I'LL  >'MPY(T  fmP1,PMI,XS,'IPS,M°S,3VIE) 

''LL  M'  OD  (XS,T  E  ip, TEMPI,  ,  OMF,  0M£» 

I A  L  L  MMP  Y  (TEMPI  ,Pr),UT,MPS»?,r>ME) 

I'LL  M'  GDIXSf  XS  ,  7EHPl,NpS,OMr,o\|E> 

-HI 


3"P5ti-ii,|£  MOVE  JO  (NFS,N-  SJj'VIE) 

^nvHJP  PROPAGATES  3  OTH  "HE  -I.TER  STATE  VECTOR  ANH  IT*1? 
I^W  rANCE  VATFIX  FROM  3  Mf  M^OA  T  E  T3  THE  N^X". 


...  7'  C’TAPLES««  « 
rMr--.ro  r f j , nfe2 » ONE 

I  T-*  HI  M  /  A  ”  RL  Y  /  XFP{d),XF30<Q),XFM(3),PFP(?,  )  ,PFPDLD<  t ,  3  )  , 

•  pf  Fo  <8, 8)  ,  ppM<3 ,8)  ,  3e3  (  3  ,8  )  ,  E  XTfJA  (f  ,b  )  ,FHI  F  (3  ,  3  )  , 

•  PHIFT(8  ,8)  ,  IJT<‘’,1)  ,  3.2p(6N,  ?)  ,R0(’  ,£)  f  T  EMF  (6, 3)  , 

t  T  E  3  PI ( 8  ,  P  )  ,  GQQM  (3,3)»R(3)jXi(  )  ,  M  ( t  ;  ,  -  )  ,  HF  (  fi  ,  3 )  , 

»  7  <  3  ,  3  ) ,  k  (3  ♦  ,f  •■ )  ,  HKARE  A  (  3  , ,  H"  (  ,  ik  )  ,  PHI  (  8,  3> 

.  .  •  c  T  L  "  E’ 9  STATE.  DRO°AGAriPH,  .  . 

I  A  L  L  SHIFTS (XFF,  XFPO ,NC3 ,nor> 

3  ALL  uHFY(XFH,OHIF,XFP, 7  PS,MP3,0ME) 


.  ..CM.TF-  CCVAf-IANCE  PR3  pAGA  TIO'J .  . . 

'ALL  rHIFT  A  (OFT- ,  PFPOLO,  MFS?,MFS?) 

I'LL  ymp> (fc  XI pA, pHIF,PF3  ,NP> ,NFS, M- 3) 

I  ALL  HMPY (Pf  FF, EXTRA , PH[ FT , Mrs, NFS ,MrS) 

3->ro=  pmj  ■  3(71-1)*  '  =HTT 

'ALL  " A  DP ( o  F  H , FP  rp , Q  FC, S  FS , M  CS , 3N  £  ) 

prk'  =  p(TI-)=pm  *  P  (  TI-1  )  *  ‘  5  3 1 T  *  3  0  ST 


-'M3 
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HOOD*  ■  O  O  O  O  O  O  D  O  *•  O  *  DO 


’:frc>'C:  F F  AML  TO  THf  ? -0  ^LIR  IMAGE  FL*S’>-. 


,rAL  TIM:  ,  A  L  FA  PT  ,  Sl  T A 0T,  XVJH,  YM-t,  7VZH,XQDi ,  YPF  T,  7  DOT,  RANG  S,  VMAX 
.  .  .  V*  =>TAFLS  uEFINIIIONS.  .. 

r:i”>  CU.kthl  TIME  IN  THE  SIMULATION,  IN  =  J~ 

’LrnT-  TRUE  A7IMUTH  PATE,  O'lTPjr  (PIXELS/:  ECONO) 

,rT-'TT-  TFIJL  ELEVATION  RAT",  OlPJf  ( =>  I  X£L  5/S  F.CoMG) 

<,Y,’''rH-  VEHICLE  POSITION  =-->om  iRAJEC,  OUTPUT  ( ML  T  E  F  S  ) 
v,v,’nnT-  VEHICLE  VELOCITY  TRAJEC  ( - L Tt F S/5EC0N0) 

t*  AY-  ,  Ol’TFUl  (PI  YELS/^FC) 

MMSr-  VEHICLF‘3  RANGE  -ROM  TH-  7RAC<SF,  jL1TPUt  (MEIERS) 

.  ..3*bf0»M  TRANSFORMATION... 

'  *  L  L  r°  M JEC  (7IM",XVEH,YVEH,"'VEH,  X  3  3  T  ,  YOOT  ,  7C0T  ) 

'HORrfXVcH  YVrH)  *  (7  VEH*/ VF-O 
?a‘|r,-=’HCF+(YVEH  YVEH) 

a LrA7r=  r’v_«j’ xcot-xveh*zoct)  ^(r-o?-  ..  :I'Z) 

?  MT3-POR7  („'  HOF  ) 

’  c  T  A  n  T  =  (FHCF*  YH0T-(YVEH*  (  (XV  EH*  <D3T«-7VEH'  ZlIOT)  /RHO  5 )  )  )  / 

!  (  PAsT-F'  .  .  .  .  2) 

3  * VO-=F0RT (TANGE) 

^X-^RT  (XPOT'  X007*  YDDT  “YODW03  T  '  7OOT)  /  r  ANG"  .  2) 

**n 


SUBROUTINE  TRAJEC  (TIME,  XV  EH,  Y  VEH  ,  Z 1 E  H  ,  XD3  T  ,  YCOT  ,  ZDOT  ) 

TRAJEC  DEFINES  THE  VEHICLE'S  TRAJECTORY  FOP  THE 
TRUTH  MODEL,  THE  TRAJECTORY  IS  3  E r I N  ED  [ N  A  3-D  REFERENCE 
FR AH F  WHOSE  ORIGIN  IS  AT  THE  TRA  C  <  ER. 

REAL  TIME  ,XVEH, YVEH,  7VE-I,  XDOT  ,Y 3  Of, ZDOT 
REAL  0T2, OMEGA  , RADIUS 

* 

C  ...VARIABLE  DEFINITIONS... 

9 

C  TIME-  CURRENT  TIME  IN  THE  SIMULATION,  IN=>UT  (SECONQS) 

C  XV  EH-  VEHICLE  POSITION  IN  THE  X-DIRECTION,  OUTPUT  (METERS) 

C  YVEH-  VEHICLE  POSITION  IN  THE  Y-D I  RED  TI ON ,  OUTpUT  (METERS) 

C  7VEH-  VEHICLE  POSITION  IN  1HE  7-DIRECTION,  OUTPUT  (METERS) 

C  XDOT-  VEHICLE  VELOCITY  IN  THE  X-DIRECTION,  OUTPUT  (METERS/SEC) 

C  Y30T-  VEHICLE  VELOCITY  IN  THE  Y-CIRECTION,  OUTPUT  (METERS/SEC) 

C  700T-  VEHICLE  VELOCITY  IN  THE  7-DIRECTION,  OUTPUT  (METERS/SEC) 

C  Of  2-  SAMPLE  RATE/2 

9 
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O  k 


C  ...OEFINE  PARAMETERS • •• 

a* 

OT  2  =  i.  /br  .U 

* 

C  ...OEFINF  TRAJECTORY... 

* 

CONSTANT  VELOCITY  CROSS  ?»NSE 

X00T  =  -53T  .33 
YOOT  =-R  3  .33 

zoor=(..o 

XV  EH=i‘0:.  .C  MXOOT*TIM£) 

YV  EH  =18  LC  ,r.  ♦(YOOT*TIME5 

7VEH=1*;0  L.*' 

EM  0 


C  . .  ,r-TrN5  [  (•  TEkS.  .. 

’j  1 2  -  ; .  /  *  . 

c  •  i. TUf  j 


O'*  -  <*,  \  = 

.  1  ?  7  •  7 

cT'ir  = 

«  '  '  .1  V* 

,.  .0  '  FT  N 

:  *  *  A  J  CTOKY  .  .. 

.  F  c  r :  h 

,  _f  .  2.  )  THEN 

XH  r  T:-'{ 

• 

Y10T=  . 

■'O  CT  ="  . 1 

XV  EH -7  . 

.  -MY  701  TIME) 

YV  rM  =  r 

« 

TV FH= j- 

• 

"L 

P'G 

IN  TIJPf 

xioT- 

.  *  CCS  (OIEGA*  (i  1  K-LT2-2. 

)  ) 

noT  :  *F 

.  ■  SI  M0-1FGA*  (T  IK  -L  K-2. 

)  ) 

■»D  or  =  ■ , 

XV  rH  =  *  r  ( 

.  -FAPI!JS*(SIN(OM-:GA*(TiMi 

_  -> 
u 

YV  rH  n 

.  «4  AOTU-J  Ml .  ■.  -COS  O  f  EMM  i 

I  ^ 

V  V  Fw  -  J  r  • 

•  ■ 

riN  PTF 
rN  0 
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*  o  •  *  o  o 


SU  PROUTINE  QTUN£(TFO>TI'4£»’'T  » FI j  F  t ( 


OTUNE  PROVIDES  TUNING  CAFA3I-LTY  FOR  THE.  FILTER 
01  SC  RET  E  NOISE  STRENGTH  MATRIX,  0-0. 

REAL  OFO(d,g) ,TIM£,OT,SIGFi, VARTO 
SA  VE  VARYO 

..  .ACQUISITION  VARYO.,. 

IF  (TJME  .LT.  f.,'4)  VAPYQ=SI  GF1 
IF  (TIME  .LT.  f,S)  VARY3=VARYQ-{.I»*SIGFI) 

C  ...FILL  CUT  FILTER  OISCRLTE  OFO  MATRIX... 

* 

0F0(l,l)=0T*,*5*  VARY0/2P. 

OFO(L,3)=OT**l.»  VARYQ/8. 

0FD(i,5)=0T'*3‘ VARYQ/6. 

Q- 0(3,2)=0F0(1 ft) 

OffC(?,fi)=QFO(i,3) 

0*  0( 2 ,6) =  QFD (1,5) 

Or  0( 3,1) =QF0<1,3> 

0PD(3,3)=DT*»-3*  VARYO/ 3. 

OcO(3,5)=DT#*2*VARYO/2. 

0*0(4, 2)  =  OFD  (  3 , 1) 

Qf  0( )=0F0(3,3) 

0*0C  ,5)=QFP(3,5) 

0‘C(C,1)=0F0(1,5) 

OrOC5,'')=QFO(3,S) 

0-0(',5)=VArY0‘  OT 
0F0(£,2)=0FD<1,5) 

OF  0( GjU ) =QFO ( 3 ,5) 

QFOCj,?  )sQFC(5,5) 

♦ 

END 


j’-P3?  IT  JP  (  GIC£(N,H) 

)TM'N?TON  W(N) 
lot'  J=1  ,N 

r-)ML=  . 

''0  3  f  si,  li 
r^’AL  =  TOtM.  ♦  RmNFO 
i  :ONTT‘"IE 

->  (  I)  =  TOTAL  -  5 . 

15  T  OvITr  MUE 

?  r  T ')  •’ M 

-  NO 
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:in?.l JTIN£  CHOL£X(A,S,M) 

1T,'»r'l',T0\'  A  (1)  ,S  (I) 

'IOt‘14.  rM  +  i 
r  P'_=1  .  c-;. 

JNs'l  MOIH 
rOLl*'. 

TO  l  I=i,NN,NDI1i 
?  =  «i>  (Mr )) 

1  r  P(3.  PT.TCJL1)  T0Li=R 

rOLl  =  T0L1» 1. £-12 
’  r  =  i 

IT  ?  T«t»N 
r*H  =  T-‘ 
it  ■  j  j=a  , in  , nhim 
r-  5  (  M)  =  . 

ro  =  T  T  +1  Ml 

j=m:  0)  -C  OT  (iHi  ,  s  (aI  )  ,S<  TT> ) 
re(503(F) «L7  t(T0L*A{I0)  +  TOLt ' )  30  TO  5 
rcR)  13,?  ,2? 

15  «3  =  -i 

?oi  r  * ,  'CHuLrs^  tr^eo  to  facto k  m  HDtF;rT? 

?  :T|P'I 

2v  '(!"}>  =  $0*1  ( F. ) 

FF(t.cn.V)  FETlHM 
.  =  T  r  *  M  0 1 H 

TO  ’*  JJ=L,  KN,fOH 

r  )=  )  i *TM1 

2‘  3(TJ)  -  (fl(lJ)-Dl3T(iNl,S(II),S(JJ)))/i(ni 

fT  =  TI*MDlM 
» 

‘•n 


jipor.r i me  kmpy  (c,a, b,<,  m,n) 

T  t‘1*,-15T0N  C  CK,N )  ,  A(<,H>  ,  OCi,M) 

10  1  T- 1,K 
10  1  J=i,N 

:  tr,  n  -  . 

1  lO'JT’VUE 

10  •?  1  =  1, K 
10  ?  J-i,N 
to  “  r  ~ 1 »  M 

:  (l*  n  =  cd,  n  ♦  (a  (l,d»ait,  ji  > 

-,0'ir’MME 

*  1J0 


! 


y at ki y  ' 
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S'nrVJTint  MAOP(  C,A,  9,J,  •'jI^LACI 
U*1-*»«T0N  i  <J,K)  ,0(J,K),C(J,<) 
l- (T^L 'G. tO. i)  30  TO  L 
DO  T  1 ,  J 
DO  5  M=l, K 

:  H,‘M  =  A(N,H)  -  0  (N,rt) 

3  ;omttmme 

'  -rijTKj 

5  10  1  N=t,J 

0"  1  U=1,K 

: fM,*)  =  A(N,M)  ♦ 

r  Do^rTf"jE. 

?  rrij->ig 


'•JO^OUTIkc.  f-WPiTE(A) 

?<-»l  "  t  e  4 ,  t  *. ) 

r  TL00><=1 

OO  f  16*1, *7,e 

r*  «»  n  =  i,rr,8 

°RI  NT  •  (  r\  2,  A,l2/>  *  ,  ‘BLOCK  1 ,  [  «.OCK 
IBLOOK=I0LOCK+1 

■»r,TNl  » <*  <3  <2X,G13. ?)/)>  *, ( (&(I, J> , J  =  1A  ,1'  ♦')  ,  l=iB,  10**  i 

Do'jrrM'jE  -  - 

r>o 


>  1 1 3  33 1 1 T 1 N ;  H0U1 ( A) 

>-M  A(t,e) 

3'3r^-'(e(a(2X,Gl3,7)/)),,((Ml,J),J  =  L,6),I  =  l,fiJ 
r  M 1 


3UaB0!JTitfr  MOUT 1  { A  ) 

3  ^  AL  A (6, 2) 

5'?rkT'(&{2(2X,  013. <)/))•  ,  <(A(I,J)|J=l,2)»i=l»P> 
•MO 


x  ’1070  ’J'Tht  MOUT  2  (  A  ) 

3  “  A  L  A  <  b ) 

3 3 7*1'  *  (£  f  2X ,  U1  2  ,  .*/)  )  * ,  ( A  (I)  »  T  =  1 »  i  > 
r  M  n 


>iip,=,T»tInE  MUUT3  (A) 

3r*l  t  <U,C‘  ) 

5°rMT  »  (R  (:*.  (2*»Gi?.7)/)>  *,  ( ( <•  (I,  J»  ,T=  i,c>  ,  J=lf  -  •  ) 
rM3 


118 


rifiE  SHIFT  (4, 9, IS,  N) 
d  r o* on  knj  ,3(.o 

C  \~  r  MPUT  tnrt-y  ,  3=uUT?J7  AR?AYH=  t RRA Y  SIZE, IS=«R, T  OF  SHI^T 

c 

NN  =  N 

DO  V  *  1=1, N 
3:,  3m  =  n. 

C  TEST  FOP  LEFT  OR  RIGHT  SHTFT 

[FtI3.LE.:  )  GO  TO  1PC  . 

C  EX -CUTE  FIGHT  SHIFT 

•*M  =  1*-TS 
H=N-*S 

CALL  SHIFTA(A(1) ,3(HH)|i,NN) 

DO  TO  2!.: 

C  EXECUTE  LEFT  SHIFT 

1.  M=l-TE 

C'LL  SHIFTA(A(HM) ,  8  ( 1 ) ,  i  ,  NN) 

2..  3  ETLP  N 
EMC 


cm  ROUTINE  SHI  FT  A  (A  ,  9,rt,  N) 
DTH“  IS  l  ON  U  (N>  ,3(N) 

DO  l  ’  K=i,H 
1.  .  D  (<)  =  A(K) 
f  "T'J'N 
"NO 


?')SRO'JTlNc  F  NOF  (  A  ,  K) 

C  S'J^RCUTIKE  TO  ROUhCOFF  A,  ~0  NEAREST  I  N  T  EG  EP  ,K 

C 

<  =  i=’i;x  ( A) 

3  =*-'< 

<  =  ’<>! 

r-(‘O.LT.  .  f  )  GO  TO  10 -J 
1  3-T'JRM 
rH0 


•'JNCTTON  DOT  ( HP  ,  A  ,  B) 
DIMENSION  A (1) ,8  (1) 
">0  T=  • 

DO  1  T=i,NS 
1  D  0 r  -  OOT+AdD'Bd) 

?  E  T  J  T  N 
E  NO 
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5  °  TO3  a  M  PLOTTER  (OUTPUT,  TAPE"  ,=>L0T,  T  A  3Et  =  3  UD  PUT) 

’-At  yALCOPR 

:  XM  (i5  2)  ,VAR(152) ,  FVA*  (if  ?)  ,  <r?vaR<  1'.  2  )  ,YMMVfcMlr  2) 

:n^^/KT*n  IX/YALC0RR  m  ,2L,  tr  > ,  srai s c- , 2.  i-:* )  .error  (2, 2,  n  ) , 
•  TrRA  (1£2),TVKL(152>  ,  VPT  (4,’*,  21) 

8 

ge-?  =  '. 
or  =  i.  /f( , 

3PwiNn  s 
gPTLr  =  ‘) 
rr^’rts! 

R  RJM=? 

:ALL  Dr,OCESS  (NF3,NRUN,NTTM:.,  OT,MFU£) 

'.all  o"t=ui  (nfs,  t, nt i me,  5, or ) 

'ALL  riP.APH(NTIM£,sjRUN) 

'T3n 

*NH 


3  UP3"'  U^  I  N’c  PROCESS  ( NFS,  ^  RUM,  NT! ME  ,  OT ,  NFI  „  E) 

REAL  •'ALCCKP 

3  OMM  ">N /Mu  T(*  I  X/^ALCCRR  (1  ♦  ,?  ,15  )  ,  ST  A)  S  (>,  3,  l’3i  )  ,  ERROR  (2,2,  t"i)  , 
'  7”"  (lsZ)  ,TYMS  (152)  ,  V3  T  (*,'.,  2 1 ) 

U  MEN  FT  ON  ESUM(S)  ,SUMSO(n)  ,3.MSiH(,) 

IT  '  T  =  1 , Nf  UN 
TO  =  J-l , NT  I  ME 

(  Kt  L  CORR  ( L,  I  ,J),L  =  1,1W 

i  ',0‘ir'M'iE 

?=rL  TAT(NRUN) 

M  =  rLO£  T  <  KRUM-i ) 
in  V>  T si, NT I ME 
Tr?0(T)  =  . 

.  •>  Y*E(  T)  =  clOAT  ( I)  'OT 
DO  l  /si, 6 
rSIH(<)  =  , 

sIHSOPO  =  '  , 

1  D  3  N  T r  N IJ  E 

in  •>  v  - 1  f  u 

=  r  . 

2  'n\|T*NUE 

D  "*  "  L-1,4 
ah  5  J  =  1 ,  HC’UN 

-R3  '  •<  A  L  C  0  f  R(L,J,I)  -  <  AL^nncj  (l*l,  j,  I) 

*FJMfL)  =  £  S II M  (  „  )  ♦  EF,= 

?'JM  3^(1.)  =  SUBSOIL)  ♦  ERR*  *  ** 

3  -  R '  J*  /  L  )  =  IMSUM(L)  ♦  <\ LCOP"*  ( L  *- 1  :  ,  J,  T  ) 

5  Z  ONTT  *J'lF 


•*0  t  1  =  1,NMJN 

■'*’1  =  KtLTCFPd,  J,I)  -  K/>L“^;>R(3,  J,i  ) 

-=>?■»  =  KtL:r<?5  (3,  j, i )  -  <ALcn*ra: ,  j, d 

'S'M(r>  =  £SUR<5)  ♦  ERR  1 
r  ^  J  ^  f  5  ^  =  c5MM(5)  ♦  ERR’ 

'HiRi ( r )  =  ruMsa (i)  ♦  tRoi-*'-’ 

=  SUMS!  ( '6 )  +  ER  R2f  *  ■* 

li  tont-nme 

TO  ?  X’sl,*- 

"M-Vl  =  LSUHCO  /R 

R  >  m  -  V  =  SOFT  (  (SLMSQ  (K) -R*  £M“AN'  *  ?) /RH) 

-  r  -  \f  =  son  (PMS'JMOO/R) 

;TU*>C<,1,I)  =  EMEAN 

■TJT'(<,e,i)  =  ESTOF.V 

'TU'K,;,I)  =  c  SIDE  V 

E .  JON^N’IE 
TO  ?*.  ‘-'=3,2: 

-MR  Vi  =  ESUf  (<  +  ♦)  /r> 

:'Tn-v  =  SORT  ( (  SU'iSO  ( K  +  ^  >  -p*  "MFSN‘  '  Z)  /PM) 

r5’0?  C<,1  ,1)  =  EHcAU 

ra’r  =  ESTQEV 

21  TOMi’r*|fJE 
s  9  TONTTM'IE 
TO  S'  L  =1 » ’» 

10  R‘  T  =  1  ,  ^ 

-  S’M'  T  )  =<AirOFF(L  ,i,  i‘  3 ’• )  -•'AlCORRt  !.♦>-  ,1 ,  1 
“IMS'!  (I)  =  ESUM(I)  "2 

M  :o^r-»!)£ 

/ar(-t‘,1)=  VPU  L,2,l)=  VFT(L,3,1>=  v/  PT  C  _ 

00  ■*  r  -  c  1 2’ 

5  =- LO  * “  (T  ) 

5-=PL0AT( X-i ) 

oo  s  i=i,»< 

:ttm-= j*3t 

r  RR=<  f  LCGF;P  <L , } , ITIKE) -< ALCO^R <-♦ « , I » ITT « 
J)=  ESUH(  J)  +ERR 
SU'ISOf.ijs  SUMSO  (  J)  +  ERR*  *  2 
•n  J=  ESIJM  (  J)  /R 

RSrOr\/=  SrKT((SJMSQ(J)-R  *  E  M  r  *•  Kj «  *i» 

/  °  T  ( '_  f  ),I>=  ESTTEV 
6 1  :ONTTM'JE 
5F  OO^Otmije 
CS>  ;ONTTN'IE 
RET' ION 
rN0 
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: MO ROUTINE  OUTPUT  (NFS, NS  TAT,  MTU i  ,  NFILE  ,DT) 
i-M  ^LCO'S 

-  o  HTi  'Mfi  T  RI  X/KALCORR  (1+  »  2  ,t3'>»STArS(‘i,7,i*.  )  ,  ERROR  ( 2  »  2»,1"  )  * 

*  T ”P")  ftEZ)  ,~Y ME  (152)  ,VPT  (*•,<»,  2i> 

TO  "  T  =  i,t‘FS 

r  T*-- "  , 

Norrr  (k,Fiir  ,d  i 

T  1  //2X  ■*•♦*»*  THr  c0LL.HINS  IS  jUTFUT  Fjt  ERROR  STflTr  , 

* r  l , ?y , *•*  *  '••,//) 

J-’TT  -  (NFTLL,2) 

-0P'1AT(2X,,,TxHF,*,3X,"MtAN  rRo0R",  =  *  ,  “STO  OF  ER-  OR”  » E  X  ,  "FILT2  o  EST  " 
*,,*7M3T'-  UF  LTTV) 

3 0  ?T  J=1,UTIHF 

riM-  =  ti  'ic >oi 

•nit : (mfile  ,3)  time, < stai -sc ,  j>  , <  =  i, nst an 
?0P^!'T(lXjFl  .  t  5  (bX,  E12.  9) ) 

JTMT’M'IE 
lOMT’ VUE 

;  rtipii 

r-n 


SUBROUTINE  GRAPH(NTIMt,NRl.'U> 

?-AL  <A  L  C  Or  F 

XM(ir2)  ,YAR  (1  EE)  ,  FV  A?  fl?2)  ,<^PVAP(lr2)  ,  VMKV  AF.  ( 15  2 ) 

'0  1mp‘I/m&T~TX/^ALC0RR(1+  » 2  ,  AO  >  ,STATS((.,?,li;f  )  ,  ERROR  ( 2  ,  2>  t  >  ^  )  > 

•  7CR'1  (152)  ,  TVMF  <  1  iZ)  ,  VPT  (‘  s'*,  21) 

3  T  •( F'l  5 1  CM  A  0(1/*  )  ,  90 (l7)  ,  00(1*')  ,00(  IV  >  ,  F0(  1.  >  tE^lM  »G0(  17)  ,40(17) 
*,  non.-)  ,R0<1?>  ,S0(1/')  ,TD  (1?) 

3  A  r  A  AH  (1 ) /2-HMcAN  ERROR  +  -t  3I5MW 
DATA  A H(3)/2  H  TARGET  0  XNAM  TCS  f 


3  A  r  A 

A  o  { f  )  /  2i  R 

X  CHANNEL 

f 

TATA 

AO (’ ) /2v  H 

FLI R  FOV 

r 

o  a  r  a 

r.o  <=  )  /2,  H 

TIME  (SEO 

r 

OA  TA 

*0(H)/2H 

PIXELS 

f 

3  A  TA 

A  0(13) /  E ' M 

y  CHANGE 

L  GY  NAM  I  Cl 

’0(1)  =  C0(1)=0C(1)=AD(1) 

30(2)  =C0(2)=D0(2)=A0(2) 

)T(;)  =  00 ( 7 ) =00  ( 7  )  =E0  (7)  =P0(7  )  =G0  (  M  =  H0  (  ’  )  =  AO  (7  ) 

50(3)  =  CO(ii)=DO(3)=EO (8)  =  FO(,)=GD<3)  =  HD(6>  =  (  D  (P  > 

30n)=r0(9)=DC(3)=E0<9>=F0m  =  GD(3>  =  HG(9)=Q0<9)  =  R0<9>=S3O)  =  T0<9) 
*10(9) 

30(1  )  =  CD  <  1  f  )  =00  (ID  =E0(  U)  =  PQ(i:  )  =  50  ( 1’  )  *M0  (1  )  =00  (  ID  =R0  (f)  = 
♦30(1  )  =T0(1'  )=A0  (!• ) 

’0(11 ) =CD(ll)  =  no (11) =EO( 11) =P0(tl) =50 (11) =^0 (1') =Q0( 11)  =R0 ( t  n  = 

*30(11) =T0(1 1 )=A3 (11) 

’0<  t’) =C0 (12) =00 ( 12) =  E0( 12)=P0(L2) =50 (12) =H0 (12) =00(12)  =R1(12)  = 
*3O(l,)=TD(12)=A0(t2) 

30  (  5)  =  =-0  (1 )  =  G0  (5  )=Q0  (*  )  =  R0(r  )  =  A0(  5 ) 

50(5)  =  CD ( 6 ) =  GO ( 5 ) =00 (G) =RP(S)  =  A3 ( 5  ) 


122 


3 

■) 

2, 


3  AM  C0C:)/2  H  Y  CHA  (NEL  / 

3O(C)=cO(:C)  =  H0(5)=SQ{5)sTO(S>=C3{5) 

A0(  3)  =^0  M)  =  HD  (o)  =SQ  (C  )  =  TC(ft  )=CD(  5) 

3AM  nO(3)/2-.H  VELOCITY  / 

30(  7)  =:.  CCi  )  =T[j(M  =B0  (3) 
mm  sr>D(C5)=T0(3)=3O  (4) 

COf  *)  =^0(  ?  )  =S0<  7)  =A0  (3) 

C0('*>  =00(?)  =S0(3)  =A0  (4) 

DAT*  nn(13)/5rn  X  CHANNEL  V E  LOC I T Y  ERROR  (S/N  =  ) 

3AM  «n(i)/2'H  ACTUAL  V 3.  FILTER  / 

TATA  m(3)/2lH  SICMA  / 

?0(L>  =  SD(i)  =  T  0  ( 1 )  =  0C(1) 

RO(2)  =  SO  ( 2 )  s  TO ( 2 )  =  00(2) 

?0(3>  =  S  0  (  3 )  =  T  0  (  3  )  =  00(1) 

me*)  =  soe )  -  tok)  =  orr*) 

3AM  00(13)  / 'JCH  Y  CHANNEL  DYNAMICS  ERROR  (S/N=  ) 

) A r A  00(13)/S*H  v  OMANNLL  VELOCITY  tRrDR  (S/N=  ) 

DATA  00(1  3) /5fM  FILLER  V  3.  ACTUAL  SIGM  PLOT  (S'N 

^  A  ~  A  2  0  ( 1 )  /  2  H  c.  E  A  N  TRACKING  -RRDR  / 

TATA  ~n(3)/2'H  FOR  FILTER  MOOET  / 

>0  (IM  =TC(1  3)  =RD (13) =00 ( 13) 

?D(L  )=TD(1*-)=RD  (  i>- )  =  C0  (  l'~) 

5D(lM=TD(lF)=cD  ( ii?)  =03(15) 

50(10)  =TC  (IE  )=p0  (lb)  =Q0(  16) 
n  ( 1’ )  =  TQ(1D  =  RD  (i7)  =00(  17) 

TATA  CD  (D/2-.  HFDR  COkRE.ATOR  NODE./ 

*0(i)  =  GO ( i )  =  HD ( 1 )  =  EO ( l > 

*0(2'  -  GO ( 2 )  =  HO ( 2  >  =  E0( 2) 

-*  V3)  =rD<3) 

'0  (4)  =  tr0(4) 

-10(3)  =00  (  3) 


HOe,)=OD(4> 

TAM  FO(13)/5LH  X  CHANNEL  FILTER  TRACKING 

3  A  "A  "0(13)/5CH  i  CHAMNE.  FILTER  I  M  C  KI :  G 


CAM  G0(l3)/ECH  x  channel  correlatcp  tracking 

3AM  HN(13)/5  H  Y  CHANNEL  CORRELATOR  TRACKING 

TALL  °  L  C  T  S  (  3  .  >  ,4LPL0T) 

CALL  ®LOT  (1 .85, -2.1, -3) 

3  A  LL  ?'MLE(TYME,7»,l5C,l) 

3  0  2  m  =  i,3,2 
30  "  T=1,NTIHE 


<'J  tT.)  =  ST  ATS (M, 1 , 1) 

'«'/'.’!I)  =  STt  T3  (M ,  1 , 1 )  +  3MT3(H,2,  I) 
✓  MK’pod)  =  S)  ATS  ( H  ,  1 ,1  >  -  <?TAT3CH,2fI) 
r  V  A  R  ( I )  =  $*ATS(M,3,I) 

V' R(* )  =  ST  ATS  H , 2, 1 ) 

CONTINUE 

CALL  =>LGT(i.?5,-2.1,-3> 

CO  TD  <6,8),M 
CALL  0-r NT  (AD,QC,1> 

CO  TO  2'. 

CALL  OTNT (COjSO, 1) 

CO  TD  2(. 

CONTINUE 

DALL  PLOT (1.73, -2.1, -3) 

CALL  pLQ"E(F) 

2  E TURN 
r  ND 


ERROR 
E  R  R  0  R 
ERROR 
ERROR 


(S/N 
(S/N 
(  S/N 
( S  /  N 


/ 


/ 

/ 

)  / 


)  / 
)  / 
)  / 
)  / 


125 


?!n?ii|Tp,£  ritJ-1  (A  ,9,IFLA  G) 

'  r  iL  <A  L  3  Or  l: 

'9 *9  TV  X*<(lf2),/AR<15  2>,  FVA  3  (1C2),^PVAR(1  2),  XMMV<-P  (15  2) 

;  T'lMTa/MaTRlx/x^LCURRd*  ,  2,  ,  V~  T  >  ,  S  TA  T  S  (  . ,  3 , 1 )  , PkROR(2 ,  2,  H  )  , 
‘  7 "’1  (!F2)  ,  TYHf{  t'52)  ,  V°r  (<>,'-,  21) 

A  (17)  ,9(17) 

z*1  ’(;  =  : )  =F7<-k  (i  51)  =XMPi/i  p(i:  u  =x“i  d-,1  )  -  '  . 

/5R(<  !;o)  =  F(/AR(152)=XMP(/AR(1"?)=<M1,/4R  (1*52)  ■  . 

'•'LL  <?'ALL(  XMPVAR,^.!'  ,3.'  2,1> 
Z'<,J7^(i:i)rXM(irl)=7£0(lrH=X1'(1/d(Hl) 

^3,/d  Z 15  2)  =XM  (16  2)  =  Z F ,< J  (i^7  >  =Xii/  (  it  2)  =-XkMVfiR(  15  2) 

5  ALL  VGF  .ft  PH (T YME , XM,  IF.  ,  A,  ,  17,4) 

'A_L  (fr,RAOH  (T  Ym£  f  <mpvar,  IF  ,1,2,15,1/0 
'/'LL  V^FAPH  (TYKE,  XrlHVA3,  if  ,  A,  2,15,1  +  ) 
f  F  (  rcL  A  G.  c.9 .  )G3  TO'  99 
'ALL  RGALEtVAP,,  .'>,3-j2,1> 

/A  5 ( » "1 ) =FVAR (151) 

7  A  ? (1“?) =  FVAk (152)=-FVA?  (IT9' 

'.ALL  VGR  A  PH  (TYPE,  VAR,  15'  >°»“",>15,d 
TALL  VGRfiPH(TY“E,FVAR,15  ,,p,  ?,l>,'d 
5  3  *  r  r  ’J'N 

“MO 


'  '  P  R">  !J  TIN  £  VGSAPH  (X,  Y,‘l,  in ,  Mn ,  fp ,  'J  >  ) 

?r4L  ^  !  LC  O'  P 

:0Ms,ri‘l  X*  Cl'..  2)  ,  VAR  (152)  ,  PVA7  (152)  ,  <1?'/£R<1  .  2),Y'‘*VAR(1:j2> 

:  n  'i'P9/Mort 1  X/X4LCORR(U  ,  2  ,  15  ’  )  ,  5  T  A  T  S  (  »  ,  3 , 1 u .  '  ,  F  RROR  ( 2  ,  2 , 1 *  )  , 
*  4  i  2  )  ,TY"f  (152)  ,  VF  r  C'  , '•  ,21) 

AI-dPTON  X(l'c)  ,Y(152)  ,  IP(17) 

rr('n.e,Q,2)  GO  TO  ?.• 

5  ALL  nL CT  (1. 15 ,-2.1, -3) 

:  all  °lot ( . .,.i,2) 

'.ALL  DLOT  (  .  ,1-  .9,3) 

: all  3lot (  .  ,ii,  ,2) 

5  ALL  ''LOT  (t  ,F  ,1  i.  ,3) 

5  ALL  r,LOT  (  > ,  1  .  9 , 2  ) 

7  A  L  L  °  L  0  "  (S.!>,.1,3) 

:all  plot  <-  ,bt  , ,  2) 

:all  nLor  r  . ,r . , 7) 

;  ALL  aLOT  (1. 3b , 1. 39, 3) 

'  ALL  DLOT  (1.35,9  .<i >,  2) 

7  ALL  °LOT  (1  .M,9.rF  ,  3) 

'ALL  PLOT  (i.»  r  ,  7  .'55, 2) 

IP  2  T  =  1  ,<■  ,  2 

'  A  _  L  ^  v  k  9  C  L  (  (i.+F+(I*1.5)*  .  I  '  ,  7  •  o  3  ,  ..  .  f  » l  Ci  ( I  )  ,  '■■  .,2i) 

2'  ;09T'kl"E 
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I'LL  =  LOT  (1..  1  ,7.  ih,  3) 

3  4  LL  PLOT  •  ,r .  5L, 2) 

;<\t_L  DLOT  (2.k5»9.53»2) 

3  ALL  DLOT(i.-'isf3.>5t2) 

:iLL  oL0T  (1 . 3- ,  9. o:»,  3) 

3  ALL  dLOT  ( •  .  •  -5  ,  9  .f>‘  ,  2) 

3ALL  PLOT (? • ^ 3 ,1, 3D, 2) 

3  A  L  L  PLOT  C.  35,1.  30,  2) 

SAIL  SVH60L(i.»Ti,l.ll5,.ti  !  ,  TOU3)  ,5'  ) 
'ALL  °LOT (i .95 ,2. 1,-3) 

/ ?=-v (U+2) 

<  1=  y  M+l  > 

<  i  =  V  (  'I  4-J  ) 

<*  =  V(‘W) 

3  ALL  ^IS<  .  ,:.,IO  (9),  -2  *,”.,3  .,*.,<2) 
'ALL  axis  <  ID  (11)  13:  ,,  Y:  ,  Y2) 

rc(sT'.rQ.-c.)G0  TO  3) 

3ALL  LTMt  <7ff-0,TYME,N,l,  ',NF) 

3  o  g  r  t  vi  •  i  £ 

3  '  LL  L  TNE  (Y  ,X,M,  1,ND,NS> 

?  ET'P'l 
r')0 


r.pPTHTif:?  VARPT  (IO,N,L) 

5  r AL  <4LCO»-fr 

X*  (ib?>  ,7  AR(152)  ,  FVA>  (l^?)  ,  <'T3V;  -M  i  2  )  ,  ymMV»R  C 1 :  ?  ) 

’•‘•/kit;  ix/<alcorr <i* L5r  > ,  stats  i  ,  3,i:  > .  > , lk?cr(2 , 2, 1 ;  ') , 

'  ■'■R1  (ir?>  ,TYH£(1  >2)  ,  VPT  (»  ,'^,21> 

3  T  X  "l|P”  ON  Y  (22)  ,10(17)  ,  RUNS  (  ,2),D.M(iM,VFlC’r  ,21) 

-:tji: /al  Escr  (ouy  ( i ) ,  vplof  ci* i  > ) 


30  f 

”=1, 

3  0  ! 

1=1 , 21 

7  o  L  0  ' 

(t,J)=  VPT(L,IfJ) 

3  0>JT* 

•7  LIE 

:  ALL 

°LOT  C  .3l  ,  -2,1,  -3) 

3  ALL 

°L  OT (  .,.1,2) 

3  ALL 

PLCT (  . ,1* . 3,3) 

3  A  L  L 

°L  OT  (  .,11.  ,2) 

3  A  L  L 

PLOT (^.L, 11., 3) 

3  A'_L 

0LUT  ,  1  J  .<9, 2) 

'ALL 

°LOT  (-.*-.,  .1 ,3) 

3  ALL 

Pl.OTC.ri,  .,2) 

:  ALL 

°LOT<  .,.  .,7) 

'ALL 

°L OT  (1  .35, 1.3:,,  3) 

3  ALL 

PLOT (1.33,9 .01, 2) 

'ALL 

PLOT  (1  .M-,  J  3) 

:  ALL 

PLOT  (1  ,-ii  ,7.  95, 2) 

'  0(1) 

=  VH  7AR1 

r  o  ( 2 ) 

=  A.  HAl.fE 

3  0  ? 

’  =  i  ,  '  ,  2 

3ALL  SYMBOL  (  (1.  i>  *(1+1.?  )»  ,D  ,7.  ii,  .  !?,ID(I),-  .,2') 
7  0'irr  MUE 
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r  P(n  =?f  H  M  £AM  ERP3  ? 

l  pi  or  (i  .o,  r.~b,  3) 

:  “'LL  =LOT  7,7.3‘;,  2) 

'ALL  PLOT  (2.-  t  ,  3  .  ,  2) 

'Ml  °LOT  (  i  .‘♦t  ,9  .  ;s>,  2) 

'ML  PLOT  (1  .  3E,  j.faS  ,  3) 

:  All  PLOT  ( ?  ,9.o5,  2) 

TMl  PLOT  <  •  !  ,  1.3i  ,  2) 

T  Ml  PLOT  (1.35-, 1.35, 2) 

■ML  pYMBOL  (l.f 5,1.115,.  1J  *  , 

(/AP1AMCE  33M\/EPGrM3  ,,TH 

'ML  p  L  0  T  ('.  .<}'  ,2. 1,-3) 

'Ml  P0AL£(rUM,4.5,et,l) 

TO  ■»  T=1,N 

P  M  x|  s  f  r  1  =FL3AT(1) 

'  o  r r  fcj '  i  e 

''JMSM'  +  i>  =URUI,S  CM2)  =3 

TMl  «YIS(  JHVARIASCC  ,ft.  ,».i  ,  1?  OIM  (  1  ) ,  PLtv  (  9  2  >  ) 

TALL  A  y  I  $  (  . ,  :  ,  , 3HRUN, -  3 , 7 . , 0  i  . , l . , 3 . ) 

MM+M  =  C »J>!  (M) 

i  (')  +  •»)  =-ouo  ( e  2 ) 

''n  ■>  )  =  i,s 

TO  5  T=? ,N 

MM-  VPLUl  (J,l) 

TOM!” MUi 

TALL  LINECT, RUNS, M, 1,1,  J) 

'OMY’miie 
>-T  TM 
EM  0 
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Appendix  E 

Performance  Plots  for  the  Extended  Kalman  Filter 
Constant  Parameters 


t 


Symbo 1 

FORTRAN 

Code 

V  a  1  ve 

0  D 

SIGSI 

0 

°f 

SIGFLR 

0 

aA 

SI  GAT 

0.2 

0g2T 

S I GMS 

1 

ARrp 

ASPRO 

5 

ISPTL 

'NO' 

NR  UN 

20 

TFINAL 

5 

SIGMFO 


Input  Parameters 


ZDOT 


HERN  ERROR  +-1  SI  CUR 


X  CHANNEL  DYNAMICS  ERROR  C5/N  =  l2.S) 


Figure  E-la 


RCTURL  VS.  FILTER 


FILTER  VS.  RCTURL  SIOMfl  PLOT  I S/N  =tZ.S  J 

Figure  E*lb 
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.00  0.80  1.60  2.40  3.20  4.00 

TIME  (SEC) 


HERN  ERROR  +-1  SI  CUR 
vELOCirr 
X  CHANNEL 
FL1R  FOV 


.80  1.60  2.40  3.20  4.00  4.80  5.60 

TIME  (SEC) 


ACTUAL  VS.  FILTER 
SIGMA 


FILTER  V5 .  RCTURL  SIG-Mfl  PLOT  (  S/N  =12.S) 


Figure  E-ld 
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BCrUflL  VS.  FILTER 


-J  r 
<x  z  x 

S  i  o 

—  r 

(OUt- 

-t 

a. 


08*0 

09  ’0  Of  0 

02  ‘0 

00’ 0° 

S13XI d 

IRL  SIO-MR  PLOT  (S/N  =12.5) 

Lire  E-2b 
36 


VELOCITY 
X  CHANNEL 
FLIR  rov 


X  CHANNEL  VELOCITY  ERROR  CS/N  =  U.5) 


Figure  E-2c 
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.00  0.80 


RCrUfiL  VS.  FILTER 


FILTER  VS.  RCTURL  SIG-MR  PLOT  ( S/N  =  12.S) 


Figure  E-2d 


HERN  ERROR  *-l  SIW1A 

forget  orNfinics 

Y  CHANNEL 
FLIR  FOV 


Y  CHANNEL  DYNAMICS  ERROR  fS/N  =  l2.fl 


Figure  E-2e 
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.DO  O.BO  1.60 


RCTUAL  VS.  FILTER 

sicnn 

Y  CHANNEL 
TARGET  DYNAMICS 


S13X I d 

FILTER  VS.  ACTUAL  SIGMA  PLOT  (  S/N  =12.51 

Figure  H-2f 
140 


r 


OO  0.80  1.60  2.40  3.20  4.00  4.80  5.60 
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nearly  identical  performance  to  the  EKF,  with  a  cost  of  four 
times  the  computational  burden.  The  constant  gain  EKF  ideal  per¬ 
formance  was  not  as  good,  but  it  was  more  robust  and  required 
only  one-tenth  the  computational  burden.  The  statistically 
linearized  filter  was  unsuccessful  due  to  the  particular 
discretization  approximation  used  in  evaluating  conditional 
expectations  inherent  in  its  structure. 
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